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ABSTRACT 


This  thesis  provides  students  with  a  set  of  graphics  tools  allowing  them  to  better 
visualize  the  effects  of  gravity-gradient  torques  on  a  rigid  spacecraft  in  a  low  earth  orbit. 
It  allows  the  user  to  select  from  a  variety  of  rigid  bodies  of  different  configurations,  place 
them  in  any  orientation  at  any  altitude,  apply  the  appropriate  gravity-gradient  moments  to 
the  body  and  immediately  see  the  effect  on  the  rigid  body.  This  is  accomplished  through 
interactive  computer  graphics  routines,  written  to  run  on  Silicon  Graphics  computers  The 
thesis  includes  a  presentation  of  the  theory  involved  in  the  programming  of  the  physical 
properties  and  then  discusses  the  basics  of  computer  graphics  including  a  more  detailed 
look  at  the  specific  implementation  for  this  thesis  A  detailed  user’s  guide  !«:  included  to 
train  students  to  use  the  tools  as  expeditiously  as  possible  It  concludes  with 
recommendations  for  further  study  in  this  area 
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1.  INTRODUCTION  AND  BACKGROUND 


A.  INTRODUCTION 

The  goal  of  this  thesis  is  to  provide  a  tool  for  the  visualization  of  the  effects  of 
gravity-gradient  disturbance  torques  on  a  rigid-body  spacecraft  in  an  orbit  around  the 
earth.  This  is  accomplished  through  the  use  of  three-dimensional  computer  graphics 
written  to  emulate  the  laws  of  physics  and  the  torques  encountered  by  a  spacecraft  in  a 
typical  earth  orbit.  The  system  is  fully  interactive  and  allows  the  user  to  study  spacecraft 
bodies  of  various  shapes  and  sizes,  including  the  Naval  Postgraduate  School's  Petite 
Amateur  Navy  Satellite  (PANSAT). 

This  thesis  begins  with  a  background  discussion  of  the  need  for  visualization  tools 
of  this  sort  It  then  goes  in  to  a  discussion  of  the  physics  of  gravity-gradient  disturbance 
torques  as  well  as  the  development  of  the  equations  for  the  gravity-gradient  moment. 
Next  it  covers  the  graphical  implementation  of  the  program.  A  discussion  of  the  results 
follows,  including  an  example  of  the  display  screen  and  a  tutorial  for  the  user.  Finally,  a 
chapter  on  conclusions  and  recommendations  is  included. 

B.  BACKGROUND 

This  thesis  was  written  to  provide  an  educational  tool  for  the  analysis  of  spacecraft 
motion  under  the  influence  of  the  earth's  gravity.  Presently,  there  exists  several  computer 
software  programs  that  can  be  programmed  to  simulate  the  dynamics  of  a  rigid  body  [Ref 
1].  There  are  some  that  are  programmed  to  simulate  spacecraft  dynamics  and  control 
[Ref  2].  There  are  routines  that  enable  the  user  to  study  spacecraft  orbits  and  ground 
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tracks  [Ref  3]  AJl  of  the  above  routines  have  one  or  more  disadvantages  Some  are 
difficult  to  work  with  and  lack  a  graphical  user's  interface  [Ref  2],  Others  give  only 
orbital  parameters  and  tell  nothing  of  what  is  physically  happening  to  the  spacecraft  [Ref 
3]  Some  display  the  spacecraft  in  a  wireframe  representation  [Ref  3]  Still  others 
provide  analysis  in  the  form  of  graphs  and  plots  of  data  [Ref  1],  After  using  some  of  the 
routines  currently  available,  it  became  obvious  that  this  was  not  the  optimum  way  to 
determine  what  is  actually  happening  to  a  spacecraft  in  orbit.  Plots  of  data  are  helpful  and 
can  yield  useful  information,  but  usually  only  after  in-depth  study  A  more  intuitive 
method  that  would  allow  a  user  to  see  the  spacecraft  in  motion  on  the  display  is  needed. 
The  software  described  by  this  thesis  was  written  in  an  attempt  to  overcome  the 
aforementioned  disadvantages  and  provide  that  intuitive  approach.  This  program  will 
allow  the  user  to  view  a  spacecraft-body  under  the  influence  of  gravity-gradient  torques. 
One  can  describe  the  spacecraft  body  as  a  block,  sphere,  cylinder  or  create  a  new 
configuration  such  as  PANSAT.  The  user  will  be  able  to  specify  the  body's  mass,  size  and 
dimension,  as  well  as  place  it  at  any  altitude  in  any  initial  orientation.  One  can  then  apply 
the  initial  orbital  rotations  and  the  gravity-gradient  torques  that  would  be  acting  on  the 
spacecraft.  This  tool  allows  the  user  to  experiment  with  different  situations  and  learn  from 
the  effects  of  those  situations.  There  is  no  better  way  to  learn  than  by  a  "hands  on" 
approach  and  tins  thesis  will  allow  the  user  to  sit  at  a  computer  screen  and  "play"  with 
different  values  and  see  immediately  the  effect  of  those  values  on  his  spacecraft. 


2 


U.  DYNAMICS 


A.  INTRODUCTION 

This  chapter  discusses  the  theory  on  the  dynamics  demonstrated  in  this  thesis  It 
begins  with  a  discussion  of  frames  of  reference,  followed  by  an  explanation  of  Euler's 
equations  of  motion  and  finishes  with  the  gravity-gradient  moment  equation.  For  the  sake 
of  brevity,  the  equations  used  will  not  be  derived  here.  For  a  derivation  of  the  stated 
equations,  see  [Ref  4,pp  106-1 12]  and  [Ref  5,p.  113]. 

B.  FRAMES  OF  REFERENCE 

The  motion  of  a  rigid  body  can  be  described  in  several  different  ways,  depending 
upon  the  frame  of  reference  used  As  a  result,  the  description  of  the  body's  motion  is  not 
complete  without  also  describing  the  frame  of  reference.  The  two  frames  of  reference 
used  in  this  thesis  are  the  orbit  reference  frame  and  the  body  reference  frame.  The  orbit 
frame  consists  of  three  orthogonal  axes,  o,,  Oj  and  Oj,  that  allows  one  to  describe  the 
motion  of  a  spacecraft  with  respect  to  its  orbit  plane.  For  the  purposes  of  this  thesis,  o, 
will  be  anti-earth  pointing,  Oj  will  be  in  the  direction  of  flight  and  o,  will  be  in  the  direction 
of  the  orbit  normal  (see  Figure  2,1).  The  body  frame  is  fixed  to  the  spacecraft's  principal 
axes  (see  Figure  2.2)  and  coincides  with  the  orbit  frame  in  the  absence  of  spacecraft  roll, 
pitch  or  yaw.  If  there  is  a  roll,  pitch  or  yaw,  these  two  frames  are  separated  by  a  rotation 
through  that  angle  about  the  appropriate  axis  (see  Figure  2.3).  One  can  describe  the 
spacecraft's  motion  in  either  frame  and  can  switch  back  and  forth  between  frames  by  using 
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a  Direction  Cosine  Matrix  (DCM)  to  convert  from  one  set  of  coordinates  to  the  other  A 


complete  discussion  of  DCM's  can  be  found  in  [Ref  6], 


o2 


Figure  2. 1 .  Orbit  Reference  Frame 


Figure  2.2.  Body  Reference  Frame  Figure  2.3.  Orbit  and  Body  Frames 
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C.  EULER'S  EQUATIONS  OF  MOTION 

Euler's  equations  of  motion  are  a  set  of  three  coupled  differential  equations  that 
describe  the  effect  of  applied  moments  on  a  rigid  body.  In  the  case  where  the  products  of 
inertia  are  zero  (principal  axes  of  moment  of  inertia),  Euler's  equations  of  motion  are  [Ref 
4.p.  112] 

H  =  (4  -  4)  1 ) 

=  4“>'  ^4  ■  4)  (3  2) 

+  t»)xa>^(4-4)  (3  3) 


or  written  as  the  dynamical  equations  of  motion  for  a  rigid  body 


^  iw  iw 


6).  =  §  +  a).ca,(^) 


^jcr  •/r 


(3.4) 

(3.5) 

(3.6) 


where 

d)x,  (i)^,  and  (bj  are  the  rates  of  change  of  the  angular  velocity  components 
about  the  x,  y,  and  z  axis,  respectively 

(Ox,  (Oy,  and  cOz  are  the  angular  velocity  components  about  the  x,  y,  and  z  axis, 
respectively 

My,  and  are  the  moments  about  the  x,  y,  and  z  axis,  respectively 

4’  4’  4  principal  moments  of  inertia  about  the  x,  y,  and  z  axis, 

respectively. 
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Furthermore.  /„  =  +  r* )  dm  . 

(•>  7) 

A,  =  L  and 

(3  8) 

/,,  =  dm 

(3  9) 

with  dm  being  a  particle  of  differential  mass  and  x,  y.  and  z  being  the  distance  from  the 
center  of  mass  to  that  mass  particle  It  should  be  noted  that  M  is  the  sum  of  all  external 
moments  on  the  spacecraft  such  as  gravity  gradient  moment,  solar  pressure  moment  and 
control  moment,  but  for  the  purpose  of  this  thesis,  we  will  only  consider  the  gravity 
gradient  moment. 

D.  GRAVITY-GRADIENT  TORQUES 

A  gravity-gradient  torque  applied  to  a  spacecraft  body  is  due  to  the  difference  in 
the  distances  between  the  various  mass  points  on  the  spacecraft  body  and  the  center  of 
mass  of  the  earth.  The  magnitude  of  this  torque  is  dependent  upon  many  factors.  These 
factors  include  the  principal  moments  of  inertia,  which  take  into  account  the  moment  arm 
of  the  point  mass  measured  from  the  center  of  mass,  the  altitude  of  the  spacecraft,  and  the 
orientation  of  the  spacecraft  with  respect  to  its  orbit  frame.  The  gravity  gradient  torque 
on  an  arbitrary  spacecraft  can  be  approximated  by  [Ref  5,p.  1 13] 


(/=  —  /jy)Cl2Cl3l  1 
(fxx  ~  fa)C'iiCi3  y  i 

Qyy  ~  Ixx)C\2C\\  2  J 


(3.10) 


where 

|ie  is  the  earth's  gravitational  constant  and  equals  398601 .395  kmVsec^ 
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A'  IS  the  orbit  radius  and  equals  the  earth  s  radius  (6^ "'8  14  kmi  plus  the 


orbit  altitude 

( '  s  are  the  direction  cosine  matrix  (DCM)  elements  used  to  express  bodv 
coordinates  m  the  orbit  frame,  where  ( ',  =  o,  • 

Substituting  the  elements  of  Eq  3  10  into  Eqs  3  4.  3  5,  and  3  6  the  dynamical  equations 
of  motion  including  gravity  gradient  torques  become 


0)x  = 

-tOytOrC/c -lyy)  ^ 

|//xr 

(3.11) 

(0^  =  1 

[^-^Uxx-Izz)CnCij  -(Hz^xUxx-Jtz)  ^ 

1  ^  ^yy 

(3.12) 

cb;  =  1 

^~^iJyy~Ixx)C\2C\i  ~  tl)iCqy(/jy  — /jcc) 

)//= 

(3.13) 

These  equations  of  motion  that  take  into  account  the  gravity  gradient  torque  are  used  in 
the  simulation 
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III.  GRAPHICS  AND  IMPLEMENTATION 


A.  INTRODUCTION 

This  chapter  will  discuss  how  the  physics  covered  in  the  previous  chapter  is 
converted  from  an  idea  and  equations  on  paper  to  computerized  graphics  on  the  screen  It 
will  begin  with  a  brief  primer  on  the  basics  of  computer  graphics  and  end  with  the  specifics 
of  the  implementation  of  the  gravity  gradient  problem 

B.  COMPUTER  GRAPHICS 

The  programs  contained  in  this  thesis  were  developed  and  tested  on  a  Silicon 
Graphics  Elan  computer.  Listed  below  are  the  minimum  hardware  and  software 
requirements  for  proper  execution  of  the  code. 

Hardware 

•  1  50  MHz  IP20  Processor 

•  FPU:  MIPS  R4010  Floating  Point  Processor  Chip 

•  CPU:  MIPS  R4000  Processor  Chip 

•  Data  Cache:  8  KB 

•  Instruction  Cache:  8  KB 

•  Secondary  Cache:  1  MB 

•  Main  Memory:  64  MB 

•  IRIS  Audio  Processor;  Revision  10 

•  Graphics  Board;  GR2-Elan 

•  Mouse 
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Software 


•  Operating  System  Silicon  Graphics  Irix  version  4  05 

•  Compiler  Silicon  Graphics  C-*-*-  Compiler  version  3  0 

It  is  important  to  note  that  a  graphics  board  capable  of  Z  Buffering  is  essential  to  the 
correct  view  of  the  graphics  screens  The  software  will  still  function  without  it  but  the 
displays  will  be  grainy  and  some  objects  may  appear  incomplete  It  should  also  be  noted 
that  the  software  will  run  on  any  operating  system  version  after  4  05 

The  world  of  computer  graphics  is  complex  and  a  full  discussion  of  the  subject  is 
beyond  the  scope  of  this  thesis  For  a  discussion  of  the  basics  of  computer  graphics  and 
how  they  are  implemented  on  Silicon  Graphics  computers  and  used  by  this  thesis,  see 
Haynes  [Ref  7,pp  16-23], 

C.  IMPLEMENTATION 

This  thesis  was  conceived  as  an  extension  of  Haynes  [Ref  7],  As  such  it  draws 
heavily  upon  the  groundwork  previously  developed.  The  goal  is  to  start  with  a  general 
routine  and  develop  it  into  a  specific  application  to  be  used  by  students  in  their  study  of 
spacecraft  attitude  dynamics.  All  coding  is  done  in  the  programming  language  C++. 

The  software  discussed  in  this  thesis  is  written  using  several  basic  tools,  some  of 
which  were  previously  developed  by  Haynes.  The  C++  programming  language  supports  a 
data  structure  called  a  class  and  this  data  structure  is  used  extensively  to  define  the 
various  objects  used  in  the  thesis.  A  class  data  structure  is  such  that  it  contains  the 
information  that  defines  the  data  structure  as  well  as  all  the  functions  that  can  operate  on 
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the  data  structure  The  advantage  to  this  is  self-contained,  re-usable  code  is  that  it  is 
resistant  to  data  corruption  by  routines  that  are  not  intended  to  have  access  to  the  data  in 
question  This  thesis  it  built  around  three  basic  classes:  the  vector3D  class,  the  matrix3x3 
class  and  the  rigid_body  class  The  vector3D  class  contains  a  three  dimensional  vector 
data  type  used  to  store  such  information  as  a  position  vector  or  velocity  vector  Included 
in  this  class  are  several  functions  used  to  perform  various  operations  on  the  vector  such  as 
vector  arithmetic  and  normalization  The  matrix3x3  class  contains  a  data  type 
representing  a  3x3  matrix  used  to  store  such  information  as  rotation  matrices  between  two 
reference  frames  Also  included  in  this  class  are  functions  used  to  perform  matrix 
operations  such  as  matrix  algebra.  Finally  the  rigid  body  class  contains  a  data  type  used 
to  represent  any  rigid  body  and  contains  information  such  as  mass,  size,  location,  velocity, 
acceleration  or  moments  of  inertia  of  the  rigid  body.  Most  of  this  information  is  contained 
in  either  the  vector3D  or  the  matrix3x3  class  within  the  rigid  body  class.  For  example, 
velocity  is  stored  in  a  vector3D  class  which  is  in  turn  stored  in  the  rigid  body  class  and  the 
inertia  matrix  is  stored  in  a  matrix3x3  class  which  is  stored  in  the  rigid_body  class.  The 
class  also  contains  functions  that  define  the  various  operations  that  can  be  performed  on 
the  rigid  body,  such  as  assigning  it  a  velocity  or  position,  rotating  it  or  changing  its  size  or 
shape.  For  a  detailed  explanation  of  these  classes  and  their  associated  functions,  see 
Haynes. 

Another  fundamental  building  block  of  this  thesis  is  the  graphics  functions  used  to 
drive  the  display.  The  graphics  functions  are  used  to  build  and  display  the  background 
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screen  as  well  as  the  objects  displayed  on  the  background  They  consist  of  routines  to 
ready  the  screen  for  display,  routines  to  change  the  position  from  which  you  are  looking 
and  the  position  to  which  you  are  looking,  routines  to  display  various  data  fields  on  the 
background  and  routines  to  display  vanous  objects  on  the  screen  such  as  rigid  bodies  or 
more  specifically,  spacecraft  These  displayable  objeas  are  graphics  models  that  are  built 
outside  of  the  software  in  this  thesis  and  then  read  into  memory  for  display  The 
construction  of  these  models  is  quite  complex  and  will  not  be  discussed  in  this  thesis  For 
a  detailed  discussion  of  these  graphics  models,  see  Zyda  [Ref  8] 

Along  with  the  graphics  models  themselves,  a  method  of  providing  motion  to  them 
is  needed  Providing  motion  for  the  displayed  objects  is  accomplished  by  the  use  of 
numerical  integrators  such  as  the  Runga-Kutta  Fourth  Order  Method  and  the  Runga>Kutta 
Adaptive  Step  Method  (the  latter  being  also  know  as  the  Runga-Kutta  Fourth/Fifth  Order 
Method)  These  integrators  take  the  current  state  of  the  object  and  the  moment  applied 
to  it  to  determine  the  next  state. 

Finally,  a  routine  is  needed  to  collect  and  drive  all  the  aforementioned  building 
blocks.  This  is  accomplished  through  a  "main"  program  that  determines  what  action  needs 
to  take  place,  when  it  needs  to  take  place  and  calls  the  appropriate  routine  to  make  it 
happen.  This  routine  is  the  "brains"  of  the  program  and  it  uses  the  classes,  graphics,  and 
integrators  as  merely  tools  to  accomplish  its  mission. 
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IV.  USER’S  GUIDE 


A.  INTRODUCTION 

A  software  package  is  useless  without  the  training  to  use  it  This  chapter  will 
provide  the  user  with  that  training  It  will  present  the  user  with  a  step-by-step  discussion 
of  how  to  use  the  gravity-gradient  visualizer  software  It  contains  the  visualizer  display 
screens  including  the  control  buttons  that  allow  the  user  full  control  over  the  simulation 
It  also  discusses  the  different  procedures  required  depending  on  the  type  of  rigid  body 
with  which  the  user  wishes  to  work 

B.  TUTORIAL 

After  logging  on  the  Silicon  Graphics  Computer,  start  the  gravity-gradient 
visualizer  software  by  typing  ggrad  at  the  UNIX  prompt  It  will  then  take  5-10  seconds 
for  the  software  to  load  and  for  the  initial  graphics  screen  to  be  displayed  This  initial 
screen  will  consist  of  a  control  window  and  a  main  display  window  The  initial  control 
window  and  the  initial  main  display  window  will  appear  as  in  Figure  4. 1 .  The  initial  values 
for  inertia,  mass,  angular  velocity  and  angular  momentum  are  displayed  in  the  main  display 
window  in  addition  to  the  initial  rigid  body  All  functions  are  accessed  by  clicking  on  the 
appropriate  field  once  with  the  left  mouse  button.  It  is  important  to  note  that  mouse  clicks 
are  valid  only  if  they  are  performed  with  the  mouse  pointer  in  the  control  window. 
Clicking  in  the  main  display  window  will  have  no  effect.  The  only  exception  to  this  is  that 
the  right  mouse  button  can  be  held  down  anywhere  on  the  screen  to  make  a  selection  to 
exit  the  program.  The  orientation  of  the  main  display  screen  is  as  follows; 
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Figure  4.1.  Display  with  "Block”  Selected. 
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the  red  axis  is  anti-earth  pointing  (o,),  the  blue  axis  is  along  the  direction  of  motion  (o,) 
and  the  black  axis  is  along  the  orbit  normal  (o,)  As  such,  the  user  is  looking  down  on  the 
orbit  plane  from  above  with  the  earth  off  the  left  side  of  the  screen  (see  Figure  2  1). 

A  number  of  options  are  available  to  the  user  when  the  initial  screen  is  displayed 
and  the  selection  of  these  options  will  be  reflected  in  the  displayed  rigid  body.  First,  the 
user  can  select  the  shape  of  the  object  he  wishes  to  display,  choosing  from  a  sphere,  cube 
or  cylinder  with  or  without  a  gravity-gradient  boom  attached  or  a  model  of  the  PANSAT 
(see  Figure  4  1).  To  make  this  selection,  the  user  should  click  on  the  word  for  the 
appropriate  shape.  The  procedures  for  the  sphere,  cube  and  cylinder  are  different  than 
those  for  a  sphere,  cube  and  cylinder  with  a  boom,  which  are  different  from  those  for 
PANSAT  These  three  cases  will  be  discussed  separately,  in  a  step-by-step  fashion. 

If  the  user  selects  a  sphere,  cube  or  cylinder,  the  procedure  would  be  as  follows: 

•  Change  the  mass  of  the  object  by  clicking  the  up  or  down  arrows  either  side  of 
the  mass  fleld.  This  will  increase  or  decrease  the  mass  by  50  kilograms  for  each 
click.  Changing  the  mass  will  result  in  the  moments  of  inertia  being  re-calculated 
and  redisplayed. 

•  Change  the  size  of  the  object  by  clicking  on  the  up  or  down  arrows  either  side  of 
the  size  field.  This  will  increase  or  decrease  the  size  by  one  meter  for  each  click. 
The  size  of  the  selected  object  can  be  changed  along  any  axis  and  changing  the 
size  will  result  in  the  moments  of  inertia  being  recalculated  and  re-displayed. 

•  Select  a  rotation  angle  which  represents  an  initial  error  in  orientation.  This  is 
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accomplished  by  clicking  the  arrows  either  side  of  the  field  marked  "Theta", 
increasing  or  decreasing  the  angle  by  five  degrees  for  each  click 

•  Select  an  altitude  for  the  rigid  body  by  clicking  the  arrows  either  side  of  the 
altitude  field,  increasing  or  decreasing  the  altitude  by  500  kilometers  for  each 
click.  This  will  cause  an  initial  angular  velocity  about  the  z  axis  to  be  calculated 
and  displayed  in  the  field  marked  "Ang  Vel". 

•  Click  on  the  button  labeled  "Rotate  Body"  to  affect  the  rotation  by  the  angle 
selected  above  in  the  field  marked  "Theta" 

•  Click  on  the  button  labeled  "Spin  Up"  to  start  the  rotation  about  the  z  axis  with 
the  angular  velocity  displayed  in  the  "Ang  Vel"  field. 

•  Click  on  the  button  labeled  "Gravity  Gradient"  to  affect  the  gravity  gradient 
moment  on  the  ri^d  body.  This  will  result  in  the  torques  being  calculated  from 
the  altitude  and  initial  orientation  and  those  being  displayed  in  the  field  marked 
"Moment". 

•  To  terminate  the  simulation  and  reset  the  initial  values,  click  on  the  button 
labeled  "Reset". 

At  first,  the  displayed  body  may  not  appear  to  be  moving.  It  is,  but  very  slowly.  This 
routine  was  built  to  display  real-time  spacecraft  motion.  As  such,  the  user  views  the 
real-time  angular  motion  of  the  spacecraft.  To  speed  up  this  displayed  motion,  an  option 
was  added  to  allow  the  user  to  increase  or  decrease  the  magnitude  of  these  values  by  a 
factor  of  ten.  This  is  accomplished  with  the  arrows  either  side  of  the  fields  marked  "Vel 
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Mag"  and  "Mom  Mag"  Thus,  the  user  can  increase  the  velocity  magnitude  and  the 
moment  magnitude  in  order  to  better  visualize  their  effects  on  the  spacecraft. 

If  the  user  selects  a  sphere,  cube  or  cylinder  with  the  gravity  gradient  boom,  the 
procedure  would  be  changed  slightly  The  control  window  and  the  main  display  window 
will  now  appear  as  in  Figure  4.2,  assuming  that  the  cube  with  a  boom  is  selected.  The 
changes  in  the  procedure  are  due  to  the  added  complexity  of  calculating  the  moments  of 
inertia  for  the  new  rigid  body.  As  a  result,  if  the  user  changes  the  mass,  the  values  are 
changed  but  not  the  moments  of  inertia.  Additionally,  the  size  field  is  now  changed  to  the 
inertia  field.  The  displayed  inertia  values  are  for  a  standard  body  of  1000  kilograms,  a  size 
of  one  meter  in  the  x,  y  and  z  directions  and  a  six  meter  massless  boom  with  a  two 
kilogram  tip  mass.  Therefore,  instead  of  changing  the  mass  and  size,  and  then 
re-computing  the  inertia,  tl.3  user  is  able  to  change  the  inertia  directly.  This  is 
accomplished  by  clicking  the  arrows  either  side  of  the  "Inertia"  field  to  increment  or 
decrement  the  inertia  by  five  kilogram-meter^  per  click.  The  remainder  of  the  procedure  is 
the  same  as  for  the  boomless  cases. 

The  final  set  of  procedures  involves  PANSAT.  The  procedures  are  similar  to 
those  for  the  rigid  bodies  v«th  a  boom  with  some  minor  exceptions.  The  similarities  are 
once  again  due  to  the  complexift'  in  calculating  the  moments  of  inertia  of  PANSAT.  The 
differences  are  due  to  the  need  for  much  greater  accuracy  in  the  inertias  and  the  much 
smaller  initial  attitude  enors  expected.  As  a  result,  the  control  window  and  the  main 
display  window  will  appear  as  in  Figure  4.3.  Note  that  the  changes  in  the  "Theta"  field 
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Figure  4.3.  Display  with  "PANSAT"  Selected. 
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will  now  be  in  tenths  of  a  degree  increments  vice  five  degree  increments  and  the  changes 
in  the  inertia  data  field  will  be  incremented  by  five  one-hundredths  of  kilogram-meter 
instead  of  one  kilogram-meter^.  In  addition  to  these  differences,  the  changes  in  altitude 
will  be  in  twenty-five  kilometer  increments  vice  500  kilometer  increments.  These  smaller 
increments  will  allow  the  user  to  make  finer  adjustments  to  more  closely  represent  the 
actual  PANSAT  mission  specifics.  All  other  procedures  are  the  same  as  for  the  case  of 
the  boomless  rigid  bodies. 

In-depth  testing  has  gone  into  the  above  procedures  with  a  goal  of  being  user 
fnendly  in  mind.  Unfortunately,  this  goal  was  at  times  compromised  slightly  in  order 
maintain  a  user  interface  consistent  with  that  defined  by  Haynes  [Ref  7]  and  to  accomplish 
the  greater  goal  of  providing  an  easy  to  use  tool  for  the  desired  analysis.  With  the 
extensive  software  features  and  this  detailed  user's  guide  usablility  will  not  be  a  problem. 

As  a  final  note,  every  attempt  was  made  to  arrive  at  values  for  the  data  fields  that 
are  applicable  to  as  many  situations  as  possible.  It  is  recognized  that  in  some  cases  the 
chosen  values  for  the  data  fields  are  not  entirely  appropriate.  It  is  not  recommended  that 
the  user  alter  this  application  in  an  attempt  to  tailor  it  to  his  specific  needs.  However,  if 
one  possesses  the  requisite  knowledge  of  graphics  and  programming  in  C-h-  to  perform 
this  a  task,  such  changes  are  possible.  Appendbc  I  contains  suggestions  to  aid  in  altering 
the  preset  data  fields. 
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V.  CONCLUSIONS  AND  RECOMMENDATIONS 

A.  CONCLUSIONS 

The  goal  of  this  thesis  was  to  provide  a  tool  for  students  to  use  in  visualizing  the 
effects  of  gravity-gradient  disturbance  torques  on  a  rigid-body  spacecraft  in  a  typical  low 
earth  orbit  After  simulating  this  same  problem  with  software  products  that  produced  data 
plots  as  results,  it  is  obvious  that  this  thesis  provides  a  better  method  of  analysis.  A  late, 
but  very  useful  addition  to  the  thesis  was  the  implementation  of  PANSAT.  The  Space 
Systems  Academic  Group  (SSAG),  charged  with  oversight  of  PANSAT,  was  very 
interested  in  the  final  results.  The  designers  of  PANSAT  were  in  need  of  a  tool  that 
would  allow  them  to  learn  how  PANSAT  would  behave  after  release  fi’om  the  Space 
Shuttle.  They  also  needed  a  way  to  determine  how  the  solar  cells  on  PANSAT  would  be 
shadowed  in  various  attitudes.  This  thesis  provides  them  with  a  tool  for  this  analysis. 
This  was  an  unexpected  benefit  of  the  thesis  and  at  the  same  time  validated  that  1)  this 
type  of  software  was  needed  and  2)  this  thesis  would  have  future  real-world  applications. 
In  this  respect,  the  goal  was  met  and  this  thesis  has  been  successful. 

B.  RECOMMENDATIONS 

There  are  several  areas  for  further  study  regarding  this  thesis.  First,  as  mentioned 
earlier,  the  moment  on  a  rigid  body  consists  of  a  gravity-gradient  moment,  a  solar  pressure 
moment  and  a  control  moment.  This  thesis  could  be  further  developed  to  include  the  solar 
pressure  moment  and  a  control  moment  in  the  disturbance  torques  experienced  by  the 
spacecraft.  In  addition  to  the  control  torque,  a  means  of  damping  the  spacecraft  control 
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could  be  added.  Another  area  for  further  study  could  be  a  feature  to  allow  the  user  to 
obtain  a  hard  copy  of  selected  data  in  order  to  reinforce  the  graphics  display. 

A  very  in-depth  follow-on  to  this  thesis  would  be  to  convert  all  the  existing  code 
from  Graphics  Library  (GL)  programming  tools  to  MOTIF  programming  tools  Under 
GL  all  of  the  displays  must  be  manually  programmed  while  MOTIF  handles  the  low-level 
programming  chores  such  as  constructing  pulldown  menus  and  data  entry  fields.  This 
would  enable  the  programmer  to  spend  his  time  on  programming  the  simulation  vice 
programming  the  basic  graphics  functions  and  would  add  some  very  useful  options  such  as 
the  ability  to  arbitrarily  specify  data  instead  of  relying  on  fixed  increments  of  data  fields. 
This  would  be  a  major  undertaking,  as  this  thesis  consists  of  several  thousands  of  lines  of 
code. 


22 


APPENDIX  A.  GRAVITY  GRADIENT  VISUALIZER  CODE 

A.  CONSTANTS  HEADER  FILE 

#ifhdef  CONSTANTS_H 
#define  CONSTANTS_H 
#include  "vector3D.H" 

const  long  double  pi  =  3. 145926536; 

const  long  double  deg  rad  =  0.0174532925,  //conversion  from  deg  to  rad 

const  long  double  mu  earth  =  398601.395;  //  earths  gravitational  constant 

//  (km^3/sec^2) 

const  long  double  earth  radius  =  6378. 14;  //  earth's  radius  (km) 

#endif 

B.  GRAVITY  GRADIENT  HEADER  FILE 

//define  BASE_H 
#include  "constants.H" 

//include  "rigid_body.H" 

#include  "menu.H" 

C.  GRAVITY  GRADDENT  SOURCE  FILE 

//include  "base.H" 

#include  <stdUb.h> 

#include  <iostream.h> 

void  mainQ 

{ 

int  section  =  0,  bypass  =  0,  NO_GO  =  0,  mx  =  0,  my  =  0,  GO  =  0,  obj  =  2, 
go  next  =  0,  axisl  =  0,  axis2  =  1,  axis3  =  2; 

matrix3x3  rotation,  matl,  mat2,  mat3; 

vectorSD  angular  velocity,  inertia,size(10,10,10),  theta(0,0,0), 

pansat_size(l  0,1 0,10),  boom_si2e(l  0,1 0,10),  reuse, 
omega_vec(0,0,0),  initial_omega(0,0,0); 

double  mass  =  0.0,  duration  =  99999999.0,  elapsed  time  =  0.0,  step  =  0.0, 
am,  ami,  am2,  am3,  mag  =  1.0,  total  time  =  0.0,  vel_mag  =  1, 
mom  mag  =  1,  rot  =  0.0,  altitude  =  0.0,  omega  =  0.0,  radius  =  0.0, 
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X  =  0,  y  =  0,  z  =  0,  mox  =  0,  moy  =  0,  moz  =  0, 

initializeO; 

initialize_menu(), 

init_control_window(); 

main_window(), 

rigid  body  cube(l),  ball(2),  cylinder(3),  pansat(60); 

rigid  body  ball_boom(71),  cube_boom(72),  cylinder_booni(73); 

rigid  body  frame(lOO),  axis(200),  reuse  body, 

reuse_body.assign_shape(cube.retum_shapeO); 

reusebody .  assign_mass(cube.  retum_mass0), 

reuse_body .  assign_size(size); 

reusebody.addaxisQ; 

reuse_body.assign_type(  1 ); 

reuse_body .  compute_inertia(); 

mass  =  reuse_body.retum_massO; 

set_target(0.0,0. 0,0.0); 
set_eye(0.0,  0.0,100); 
settimeQ; 

while  (section  !=  99)  //  while  exit  not  selected 

{ 

section  =  queue_test0;  H  what  type  of  interrupt  event 

set_delta0; 

view(); 

//  draw  the  controls  screen 

gyro_controls(x,y,z,obj,altitude,size,theta,mass,mox,moy,moz,  velmag, 
mom  mag,  elapsed  time,  inertia); 

if  (bypass  >  0  &&  bypass  <  6)  //  system  delay  for  mouse  input 

//timing 

bypass-H-; 

else 

bypass  =  0; 

if  (section  >  99999)  //  if  the  event  was  a  mouse 

//  selection 

{ 

mx  =  section  /  100000;  //  decode  mouse  x  &  y  coords 

my  =  section  -  (mx  *  100000); 
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if  (!  bypass) 

{ 

bypass  =  1 ; 


if  (obj  =  7)  //  PANSAT 

{ 


//  Inertia  Decrease 
if(mx>  113&&nix<  129) 

{ 

if  (my  >  936  &&  my  <  953)  //  Ixx 
if  (inertia[0]  >  0.05) 

inertia[0]  =  inertia[0]  -  0.05; 

if  (my  >  923  &&  my  <  937)  //  lyy 
if  (inertia[l]  >  0.05) 

inertia[l]  =  inertia[l]  -  0.05; 

if(my>909&&my<924)  //Izz 
if  (inertia[2]  >  0.05) 

inertia[2]  =  inertia[2]  -  0.05; 

reuse  body.  assign  mertia(inertia); 

} 

//  Inertia  Increase 

if  (mx  >  165  &&  mx  <  181) 

{ 

if  (my  >  936  &&  my  <  953)  //  Ixx 

inertia[0]  =  inertia[0]  +  0.05; 

if  (my  >  923  &&  my  <  937)  //  lyy 

inertia[l]  =  inertia[l]  +  0.05; 

if  (my  >  909  &&  my  <  924)  //  Izz 

inertia[2]  =  inertia[2]  +  0.05; 

reuse_body.assign_inertia(inertia); 

} 


else  if  ((obj  >  3)  &&  (obj  <  7))  //  boom  object 

{ 
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//  Inertia  Decrease 

if  (mx  >113  &&  mx  <  129) 

{ 

if  (my  >  936  &&  my  <  953)  //  Ixx 

if  (inertia[0]  >  1) 

inertia[0]  =  inertia[0]  -  5, 

if  (my  >  923  &&  my  <  937)  //  lyy 

if  (inertia[l]  >  1) 

inertia[I]  =  inertia[l]  -  5; 

if  (my  >  909  &&  my  <  924)  //  Izz 

if  (inertia[2]  >  1 ) 

inertia[2]  =  inertia[2]  -  5; 

reuse_body.assign_inertia(inertia); 

} 

//  Inertia  Increase 

if  (mx  >  165  &&  mx  <  181) 

{ 

if  (my  >  936  &&  my  <  953)  //  Ixx 

inertia[0]  =  inertia[0]  +  5; 

if  (my  >  923  &&  my  <  937)  //  lyy 

inertia[l]  =  inertia[l]  +  5; 

if  (my  >  909  Sc&  my  <  924)  //  Izz 

inertia[2]  =  Lnertia[2]  +  5; 

reuse_body.assign_inertia(inertia); 

} 


else  //  cube,  sphere,  or  cylinder 

{ 


//  Size  Decrease 

if  (mx  >113  &&  mx  <  129) 

{ 

if  (my  >  936  &&  my  <  953)  //  size  along  x  axis 

if  (size[0]  >  1 .0) 
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size[0]  =  size[0]  -  1 , 


if  (my  >  923  &&  my  <  937)  //  size  along  y  axis 

if  (size[l]  >10) 

size[  1  ]  =  size[  1  ]  -  1 ; 

if  (my  >  909  &&  my  <  924)  //  size  along  z  axis 

if  (size[2]  >  1 .0) 

size[2]  =  size[2]  -  1; 

reusebody .  assign_size(size); 
reuse_body .  compute_inertia(); 


//  Size  Increase 

if  (mx  >  165  &&  mx  <  181) 

{ 

if  (my  >  936  &&  my  <  953)  //  size  along  x  axis 

size[0]  =  size[0]  +  1; 

if  (my  >  923  &&  my  <  937)  //  size  along  y  axis 

size[l]  =  size[l]  +  1; 

if  (my  >  909  &&  my  <  924)  //  size  along  z  axis 

size[2]  =  size[2j  +  1; 

reusebody .  assign_size(size); 
reuse_body.compute_inertiaO; 

} 

} 


//  Mass  Decrease 

if(obj  =  7)  //PANS  AT 

{ 

if  ((mx  >113  &&  mx  <  129)  &&  (my  >  866  &&  my  <  883)) 

{ 

if  (mass  >31.0) 
mass  =  mass  -  1; 

reusebody .  assign_mass(mass); 
reusebody.  computeinertiaO; 

} 
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//  all  other  shapes 


1 

else 

{ 

if  ((mx  >113  &&  mx  <  129)  &&  (my  >  866  &&  my  <  883)) 

{ 

if  (mass  >  50.0) 
mass  =  mass  -  50; 

reusebody .  assign_mass(mass), 
reusebody .  compute_inertia(); 

} 

} 

//  Mass  Increase 

if(obj  =  7)  //PANSAT 

{ 

if  ((mx  >  165  &&  mx  <  181)  &&  (my  >  866  &&  my  <  883)) 

I 

mass  =  mass  +  1; 
reuse_body.assign_mass(mass); 
reuse_body.  computeJnertiaQ; 

} 

} 

else 

{ 

if  ((mx  >  165  &&  mx  <  181)  &&  (my  >  866  &&  my  <  883)) 

{ 

mass  =  mass  +  50; 
reuse_body.assign_mass(mass); 
reusebody .  computeinertiaO; 

} 

} 

//  Rotation  Angle  Decrease 
if  (mx  >  379  &&  mx  <  396) 

{ 

if  (my  >  936  &&  my  <  953)  //  angle  about  x 

{ 

if(obj  =  7) 

theta[0]  =  theta[0]  -0.1; 
else 
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theta[0]  =  (int  (theta[0])  -  5)  %  360, 


if  (my  >  923  &&  my  <  937)  //  angle  about  y 

{ 

if  (obj  =  7) 

theta[  1  ]  =  theta[  1  ]  -  0  1 ; 
else 

theta[l]  =  (int  (theta[l])  -  5)  %  360; 

} 

if  (my  >  909  &&  my  <  924)  //  angle  about  z 

{ 

if  (obj  —  7) 

theta[2]  =  theta[2]  -  0  1; 
else 

theta[2]  =  (int  (theta[2])  -  5)  %  360; 

} 

} 


//  Rotation  Angle  Increase 
if  (mx  >  433  &&  mx  <  448) 

{ 

if  (my  >  936  &&  my  <  953)  //  angle  about  x 

{ 

if  (obj  =  7) 

theta[0]  =  theta[0]  +  0.1; 
else 

theta[0]  =  (int  (theta[0])  +  5)  %  360; 

) 

if(my  >  923  &&  my  <  937)  //  angle  about  y 

{ 

if  (obj  =  7) 

theta[l]  =  theta[l]  +  0.1; 
else 

theta[l]  =  (int  (theta[l])  +  5)  %  360; 

} 

if(my  >  909  &&  my  <  924)  //  angle  about  z 

{ 
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if  (obj  —  7) 

theta[2]  =  theta[2]  -*-0  1, 

else 

theta[2]  =  (int  (theta[2])  -t-  5)  %  360, 

} 

} 

//  Angular  Velocity  Magnitude  Decrease 
if  ((mx  >  1 86  &&  mx  <  202)  &&  (my  >  866  &&  my  <  883)) 
velmag  =  velmag  / 10; 

//  Angular  Velocity  Magnitude  Increase 
if  ((mx  >  253  &&  mx  <  269)  &&  (my  >  866  &&  my  <  883)) 
vel_mag  =  vel  mag  *  10; 

//  Moment  Magnitude  Decrease 

if  ((mx  >  279  &&  mx  <  296)  &&  (my  >  866  &&  my  <  883)) 
mommag  =  mommag  / 10; 

//  Monment  Magnitude  Increase 

if  ((mx  >  346  &&  mx  <  361)  &&  (my  >  866  &&  my  <  883)) 
mommag  =  mom_mag  *  10; 

//  Altitude  Decrease 

if  ((mx  >  373  &&  mx  <  388)  &&  (my  >  866  &&  my  <  883)) 

{ 

if  (obj  =  7) 

{ 

if  (altitude  >  0) 

altitude  =  altitude  -  25; 

else 

{ 

if  (altitude  >  0) 

altitude  =  altitude  -  500; 

} 

//  compute  inertial  angular  velocity  (radians/sec) 
if  (altitude  !=  0) 

{ 

radius  =  earth_radius  altitude; 

omega  =  sqrt(mu_earth  /  (radius*radius*radius)); 

omega_vec[2]  =  omega;z  =  omega_vec[2]; 
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omega  =  0, 

omega_vec[2]  =  0,2  =  0. 


else 

> 


} 

} 

//  Altitude  Increase 

if  ((mx  >  438  &&  mx  <  456)  &&  (my  >  866  &&  my  <  883)) 


if  (obj  —  7) 

{ 

if  (altitude  <  99975) 

altitude  =  altitude  +  25, 

} 

else 

{ 

if  (altitude  <  99500) 

altitude  =  altitude  +  500; 

\ 

I 

I I  compute  inertial  angular  velocity  (radians/sec) 
radius  =  earthradius  +  altitude; 

omega  =  sqrt(mu_earth  /  (radius*radius*radius)); 
omega_vec[2]  =  omega; 
z  =  omega_vec[2]. 


//Select  Shape 

if  (mx  >  19  &&  mx  <  103) 

{ 

if  (my  >  936  &&  my  <  953)  //  select  ball 

{ 

obj-  1; 

reuse_  body .  assign_shape(ball .  retumshapeO); 

reuse_body.assign_type(2); 

reusebody .  assign_size(size); 

reusebody .  assign_mass(ball .  retum_mass0); 

reuse_body.compute_inertiaO; 

mass  =  reuse_body.retum_massO; 

} 
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if  (my  >  923  &&  my  <  937)  //  select  cube 


obj  =  2; 

reu  sebody .  assign_shape(cube .  retum_shape( ) ) . 

reuse_body,assign_type(  1 ), 

reusebody .  assign_size(  si  ze); 

reuse_body.assign_mass(cube.retum_mass()); 

reusebody .  compute_inertia() ; 

mass  =  reuse_body.retum_mass(); 

if  (my  >  909  &&  my  <  924)  //  select  cylinder 

{ 

obj  =  3, 

reuse_body.assign_shape(cylinder.retuni_shapeO); 

reusebody .  assign_type(3 ); 

reusebody .  assign_size(size) ; 

reusebody .  assign_mass(cylinder .  retummassO) ; 

reusebody .  computeinertiaQ; 

mass  =  reuse_body.retum_massO; 

} 

if  (my  >  897  &&  my  <  910)  //  add  boom  to  existing  body 

{ 

if  (obj  =1)  //  sphere  and  boom 

{ 

obj  =  4; 

reuse_body.assign_shape(ball_boom.retum_shapeO); 
reuse_body.assign_type(71);size  =  boomsize; 
reuse_body.  assign_size(size); 
reuse_body.assign_mass(ball_boom.retum_massO); 
reuse_body.assign_inertia(ball_boom.retum_inertia()); 
inertia  =  reuse_body.retum_inertiaO; 
mass  =  reusebody.retummassO, 

} 

if  (obj  =  2)  //  cube  and  boom 

{ 

obj  =  5; 

reuse_body.  assign_shape(cube_boom .  retumshapeQ); 
reuse_body .  assign_type(72); 
size  =  boomsize; 
reuse_body.assign_size(size); 
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reuse_body.assign_mass(cube_boom.retum_mass()), 
reuse_body.assign_inertia(cube_boom.retum_inertia()), 
inertia  =  reuse_body.retum_inertia(); 
mass  =  reuse_body.retum_mass(); 

} 

if  (obj  =  3)  //  cylinder  and  boom 

{ 

obj  =  6; 

reuse_body.assign_shape(cylinder_boom.retum_shape()); 

reuse_body.assign_type(73) 

size  =  boom_size; 

reusebody .  assign_size(size); 

reuse_body.assign_mass(cylinder_boom.retum_mass()); 
reuse_body.assign_inertia(cylinder_boom.retum_inertiaO); 
inertia  =  reuse_body.retum_inertia(); 
mass  =  reuse_body.retum_mass(); 

} 

if  (my  >  884  &&  my  <  898)  //  select  PANSAT 

{ 

obj  =  7; 

reuse_body.assign_shape(pansat.retum_shapeO); 

reusebody .  assign_type(60); 
size  =  pansatsize; 
reusebody .  assign_size(size); 

reuse_body.assign_mass(pansat.retum_massO); 
reuse_body.assign_inertia(pansat.retum_inertiaO); 
inertia  =  reuse_body.retum_inertiaO; 
mass  =  reusebody.retummassO; 
theta.zeroO; 

altitude  =  0; 

} 


//  Rotate  Body  button  selected 
if  (mx  >  640  &&  mx  <  707  &&  my  >  890  && 
my<959&&  !NO_GO) 

{ 

//  compute  the  DCM 

matl  .DCM_x_rotation(theta[0]  *  (pi  / 180)); 
mat2.DCM_y_rotation(theta[l]  *  (pi  /  180)); 
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mat3  .DCM_z_rotation(theta[2]  *  (pi  /  180)); 
rotation  =  rotation  *  mat3, 
rotation  =  rotation  *  niat2; 
rotation  =  rotation  *  mat  1 ; 

//  initial  spin  is  a  function  of  the  inertial  angular  velocity 
//  about  the  3  axes 

initial  omega  =  rotation  *  omega  vec; 

X  =  initial_omega[0]; 
y  =  initial_omega[l]; 
z  =  initial_omega[2]; 

GO  =  21; 
step  =  0.0; 


//  Inertial  Moment  button  selected 
if  (mx  >  740  &&  mx  <  813  &&  my  >  890  && 
my<959&&  !NO_GO) 

{ 

GO=  11; 

elapsedtime  =  0.0; 
step  =  0.0; 


//  Spin  Up  button  selected 
if  (mx  >  840  &&  mx  <  907  &&  my  >  890  && 
my  <  959  &&  !NO_GO) 

{ 

GO=  1; 

NO_GO=  1; 


if(GO  =  2) 
NO_GO  =  0; 


//  RESET  button  selected 

if  (mx  >  940  &&  mx  <  1007  &&  my  >  890  &&  my  <  959) 

{ 

reusebody .  zeroQ; 
reuse_body.assign_su»(size); 
mass  =  reusebody.retummassO; 
inertia  =  reusebody.retuminertiaO; 
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x  =  0; 
y  =  0, 
z  =  0; 
mox  =  0, 
moy  =  0; 
moz  =  0; 
velmag  =1.0; 
mommag  =1.0, 
theta.zeroO; 

GO  =  0; 

NO_GO  =  0; 
step  =  0.0; 
elapsedtime  =  0.0; 
altitude  =  0; 
omega  =  0; 
omega_vec.zero(); 
initial_omega.zero0; 
matl.resetQ; 
mat2.reset0; 
mat3.reset(); 
rotation.  resetQ; 

} 

} 

} 

mainwindowQ; 

//  this  section  rotates  the  body  from  its  inertial  frame 
if(GO  =  21)  //  reparation  for  1st  rotation 

{ 

reuse  =  reuse  *  0; 
if(theta[0]) 

reuse[axisl]  =  .3  *  (theta[0]  /  fabs(theta[0])); 

//  set  ang  velocity 

rot  =  theta[0]  *  (pi  / 180)  /  2; 

GO+^; 
gonext  =  0; 


if(GO  ==  22)  //  animates  body 

{ 

if(go_next)  //  stop  body  for  second  rotation 
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{ 


GO++; 

reusebody .  assign_ang_velocity_bc(0,0,0); 

} 

else 

{ 

if((theta[0]  >  0  &&  rot  >  .3  *  read_deltaO)  ||  (theta[0]  <  0  && 
rot  <  -.3  *  read  deltaO)) 

{  //  rotation  incomplete 

reuse_body.assign_ang_velocity_bc(reuse); 
rot  =  rot  -  reuse[axisl]  *  read  deltaQ; 

} 

else  //  finish  1st  rotation 

{ 

if(theta[0]) 

reuse  =  reuse  *  (rot  /  (reuse[axisl]  *  read  deltaQ)); 
reuse_body.assign_ang_veIocity_bc(reuse); 
gonext  =  1; 

} 

} 

) 

if(GO  =  23)  //  prep  for  2nd  rotation 

{ 

reuse  =  reuse  *  0; 
if(theta[l]) 

reuse[axis2]  =  .3  *  (theta[l]  /fabs(theta[l])); 
rot  =  theta[l]  *  (pi  /  180)  /2; 

GO++; 
gonext  =  0; 

) 

if(GO  =  24)  //  animate  2nd  rotation 

{ 

if(go_next)  //  stop  for  3rd  rotation 

{ 

GCH-+; 

reuse_body.assign_ang_velocity_bc(0,0,0); 

} 

else 
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{ 

if((theta[l]  >  0  &&  ret  >  ,3  *  read_delta())  ||  (theta[l]  <  0  && 
rot  <  -.3  *  read  deltaO)) 

{ 

reuse_body.assign_ang_velocity_bc(reuse), 
rot  =  rot  -  reuse[axis2]  *  read_delta(); 

} 

else  //  finish  2nd  rotation 

{ 

if(theta[l]) 

reuse  =  reuse  *  (rot  /  (reuse[axis2]  *  read_delta())); 
reuse_body.assign_ang_velocity_bc(reuse); 
go_next  =  1 ; 

} 

} 

} 

if(GO  =  25)  //prep  for  3rd  rotation 

{ 

reuse  =  reuse  *  0; 
if(theta[2]) 

reuse[axis3]  =  .3  *  (theta[2]  /  fabs(theta[2])); 
rot  =  theta[2]  *  (pi  / 180)  /  2; 

GO++; 
go  next  =  0; 

} 

if(GO  =  26)  //  animate  3rd  rotation 

{ 

if(go_next) 

{ 

reuse  body.assign  ang  velocity  bc(0.0,0); 

GO++; 

} 

else 

{ 

if((theta[2]  >  0  &&  rot  >  .3  *  read  deltaQ)  II  (theta[2]  <  0  && 
rot  <  -.3  *  read_deltaO)) 

{ 

reuse_body.assign_ang_velocity_bc(reuse); 
rot  =  rot  -  reuse[axis3]  *  read_deltaO; 
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else 


//  finish  3rd  rotation 


{ 

if(theta[2]) 

reuse  =  reuse  *  (rot  /  (reuse[axis3]  *  read_delta())); 
reuse_body.assign_ang_velocity_bc(  reuse); 
gonext  =  1; 

} 

} 

} 

if  (GO  >11)  //for  rotations  only 

{ 

reuse_body.update_state_rk4(); 

reusebody.displayO; 

} 


if  (GO  =1)  //  spin  body 

{ 

reuse_body.assign_ang_velocity_bc(x  *  vel_mag,  y  *  vel_mag, 

z  *  vel_mag); 

GO++, 

} 

if  (GO  =11)  //set  moment  in  ineTtial  coordinates 

{ 

mox  =  3  *  omega  *  omega  *  ((reuse_body.retum_inertia0)[2]- 

(reuse_body.retum_inertiaO)[l])  *  rotation[l]  *  rotation[2]; 

moy  =  3  *  omega  *  omega  ♦  ((reuse_body.retum_inertiaO)[0]- 

(reuse_body.retum_inertia0)[2])  *  rotation[0]  *  rotation[2]; 

moz  =  3  *  omega  *  omega  *  ((reuse_body.retum_inertiaO)[l]- 

(reuse_body.retum_inertiaO)[0])  *  rotation[l]  *  rotation[0]; 

reuse_body.assign_moment(mox  *  mom  mag,  moy  *  mom  mag, 

moz  •  mom  mag); 


elapsedtime  =  elapsedtime  +  step; 

} 


//  apply  moment 


if  ((GO  ==  11)  &&  duration  <  0) 

{ 

GO  =  0; 

NOGO  =  0; 

} 

frame.  displayO, 

if  (duration  >  0  &&  duration  <  step)  //  last  integration  step 

set_delta(durat  ion ) , 

step  =  reuse_bodyupdate_state_rk45(. 000001), 
reusebody. displayO, 

angularvelocity  =  reusebodyretumangvelocitybcQ; 
inertia  =  reuse  body  retum  inertiaO; 

ami  =  angular_velocity[0]  *  inertia[0]; 
am2  =  angular_velocity[l]  *  inertia[l]; 
am3  =  angular_velocity[2]  *  inertia[2]; 
am  =  sqrt(aml  *  ami  +  am2  *  ain2  +  am3  *  ani3); 

//  display  statistics 

stat_controls(angular_velocity[0],angular_velocity[l],angular_velocity[2], 
am,reuse_body.retum_massO,  (reuse_body.retum_inertiaO)[0], 
(reuse_body.retum_inertiaO)[  1  ],(reuse_body.retum_inertia0)[2]); 
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APPENDIX  B.  GRAPHICS  CODE 


A.  HEADER  FILE 

#ifndefGRAPHICS_H 
#define  GRAPHICS_H 
^include  <inath.h> 

#include  "vectorSD.H" 

#include  "matrix3x3.H" 

#include  "quaternion. H" 

#inc!ude  "rdobj  opcodes.h" 

#include  "rdobj_funcs.h" 

#inc!ude  <stdio.h> 

#include  <gl.h> 

#include  <device.h> 

//initializes  the  graphic  system 
void  initializeO; 

//initializes  control  window 
void  init_control_window(); 

//make  viewing  window  active 
void  main_windowO; 

//  makes  control  window  active 
void  control_window(); 

//clears  control  window 
void  clear_control_window(); 

//control  window  for  euler  program 

void  euler_controls(int,  int,  int,  int,  int,  int,int,quatemion,  double); 

//control  window  for  gyro  program 

void  gyro_controls(double,  double,  double,  int,double,  vertor3D,  vectorSD,  double, 

double,  double,  double,double,  double,  double,  vector3D); 

//statisic  display  for  gyro  program 

void  stat_controls(double,  double,  double,  double,double,  double,  double,  double); 
//control  window  for  frame  program 

void  frame_controls(int,  int,  vectorSD,  vector3D,vector3D,  int); 
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//standard  function  for  viewing  a  scene 
void  view(), 

//used  to  view  the  scene  for  a  point  of  view  fixed  to  a  rigid  body 
void  view(quatemion,  vectorSD,  int), 

//attaches  the  eye  to  a  rigid  body 
void  attach_eye_to(vector3D*,  int*), 

//attaches  the  tatget  to  a  rigid  body 
void  attach_target_to(vector3 D * ) ; 

//attaches  the  eye  to  a  rigid  body 
void  set_eye_to(double,  double,  double); 

//attaches  the  target  to  a  rigid  body 
void  set_target_to(double,  double,  double); 

//rotates  the  view  in  tenths  of  degrees 
void  rotate_view(int); 

//displays  the  body  axes  of  a  rigid_body 
void  view_axis(); 

//gravity  check  -  returns  non  zero  value  when  gravity  is  turned  on 

int  gravitystatusQ; 

void  set^avity_onO; 

void  set^avity_offO; 

void  togglejgravityO; 

//air  resistance  check  -  returns  non  zero  value  when  air  resistance  is  turned  on 

int  airresistancestatusO; 

void  set_air_resistance_onO; 

void  set_air_resistance_oflfO; 

void  toggle_air_resistanceO; 

//  c  routines  the  must  be  accessed 
extern  "C" 

{ 

extern  OBJECT*  read_object(char[]); 

extern  void  ready_object_for_display(  OBJECT*  ); 

extern  void  display_this_object(  OBJECT*  ); 
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#endif 


B.  SOURCE  FILE 

#ifiidefGRAPfflCS_C 
#define  GRAPHICS_C 
#include  "graphics.H" 

#define  NEARDEPTH  0x000000  /*  the  near  and  far  planes  used  for  Zbuffering*/ 

#define  FARDEPTH  0x7fffif 

OBJECT  *lightobj,  *axis, 

//eye  and  target  are  the  global  variables  that  control  the  view  point 
//and  reference  point  of  the  scene  respectively 

vectorSD  *eye  =  new  vector3D(10.0,  10.0,  10.0),  ^target  =  new  vectorSD; 

int  *eye_display_field  =  NULL; 

int  gravity  flag  =  0,  air  resistance  flag  =  0; 

int  twist  =  0; 

long  main  win,  control_win; 

Matrix  un  =  (  1.0,  0.0,  0.0,  0.0, 

0.0,  1.0,  0.0,  0.0, 

0.0,  0.0,  1.0,  0.0, 

0.0,  0.0,  0.0,  1.0}; 


int  gravitystatusQ 

{ 

return  gravity  flag; 

} 


void  set ^avity  onO 

{ 

gravity  flag  =  1; 

} 


void  set^avity  offQ 

{ 
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gravityflag  =  0, 


void  toggle_gravity() 

{ 

if(gravity  flag) 

{ 

gravityflag  =  0; 

} 

else 

{ 

gravityflag  =  1 ; 

} 

} 


int  airresistancestatusQ 

{ 

return  airresistanceflag; 

} 


void  set_air_resistance_onO 

{ 

air_resistance_flag  =  1; 

} 


void  set_air_resistance_oflfO 

{ 

airresistanceflag  =  0, 

} 


void  toggleairresistanceO 

{ 

if  (air  resistance  flag) 

{ 

air_resistance_flag  =  0; 

} 

else 

{ 
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air  resistance  flag  =  1 , 


void  initializeO 

{ 

/*  set  up  the  preferred  aspect  ratio  */ 
keepaspect(XMAXSCREEN+ 1 ,  YMAXSCREEN+ 1 ); 
prefsize(XMAXSCREEN/2,YMAXSCREEN/2), 
prefposition(0,XMAXSCREEN  *  0.8  ,0,YMAXSCREEN  *  0.8), 

/*  open  a  'Aandow  for  the  program  */ 
mainwin  =  winopen("Main"); 

vvintitle(’' Gravity-Gradient  Visualizer,  By  Jeff  Stewart"); 

/*  put  the  IRIS  into  double  buffer  mode  */ 
doublebufferO, 

/*  put  the  iris  into  rgb  mode  */ 

RGBmodeO, 

/*  configure  the  IRIS  (means  use  the  above  command  settings)  */ 
gconfigO, 

/*  set  the  depth  for  z-bufifering  */ 
lsetdepth(NEARDEPTH,FARDEPTH); 

/*  queue  the  redraw  device  */ 
qdevice(REDRAW); 

/*queue  the  menu  button*/ 
qdevice(MENUBUTTON); 

/*tum  the  cursor  on*/ 
cursonO; 

/*select  gouraud  shading*/ 
shademodel(GOURAUD); 

/*tum  on  the  new  projection  matrix  mode*/ 
mmode(MVIEWING); 


45 


/*Tum  on  Zbuffering*/ 
zbuffer(TRUE); 

lightobj  =  read_object("the_light.off’), 
axis  =  read_object("axis.ofr'); 
ready_object_for_display(lightobj); 
ready  _object_for_display(axis); 

//queue  up  input  devices 

qdevice(LEFTMOUSE); 

qdevice(RIGHTMOUSE); 

qdevice(MOUSEX); 

qdevice(MOUSEY), 

qdevice(RIGHTARROWKEY), 

qdevice(LEFT  ARROWKE  Y), 

qdevice(UPARROWKEY); 

qdevice(DOWNARROWKEY); 

qdevice(SPACEKEY); 

qdevice(EQUALKEY); 

qdevice(MINUSKEY); 

qdevice(FlKEY); 

qdevice(F2KEY); 

qdevice(F3KEY); 

qdevice(F4KEY); 

qdevice(F5KEY); 

qdevice(F6KEY); 

qdevice(F7KEY); 

qdevice(F8KEY); 

qdevice(F9KEY); 

qdevice(F  1 OKE  Y); 

qdevice(Fl  IKEY); 

qdevice(F12KEY); 

//clear  draw  and  display  buffer 

czclear(0xFFFF7200,getgdesc(GD_ZMAX)); 

swapbuflfersO; 

czclear(0xFFFF7200,getgdesc(GD_ZMAX)); 


void  init  control  windowQ 

{ 

/*  set  up  the  preferred  aspect  ratio  */ 

prefposition(0,XMAXSCREEN  *  0.8,YMAXSCREEN  *  0.87,  YMAXSCREEN); 
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/*  open  a  window  for  the  program  */ 
controlwin  =  winopenC'control"), 
wintitle("  System  Control  Window"), 

/*  put  the  IRIS  into  double  buffer  mode  •/ 
doublebufferO; 

RGBmodeO; 

gconfigO; 

pushmatrixQ; 

ortho2(0.0,  769.0,  0.0,  100.0), 

RGBcolor(255,255,255); 

clear(); 

popmatrix(); 

swapbuffersO; 

} 


void  main_window() 

{ 

winset(main_win); 

} 


void  control_window0 

{ 

winset(control_win); 

} 


void  clear  control  windowQ 

{ 

winset(control_win); 

pushmatrixO; 

ortho2(0.0,  769.0,  0.0,  100.0); 

RGBcolor(255,255,255); 

clearQ; 

popmatrixO; 

swapbuffersO; 

winset(main_win); 
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\  oid  gyro_controls(double  x,  double  y,  double  z,  int  object,  double  altitude,  vector3D 
size,  vectorSD  theta,  double  mass,  double  tl,  double  t2,  double 

t3,  double  vel_mag,  double  mom_mag,  double  elapsed, 

vector3D  inertia) 


{ 

float  pt  [3][2]  =  {  {142,48}, 
{148,48}, 
{145,42}}; 
on 

float  pt2  [3][2]  =  {  {182,42}, 
{188,42}, 
{185,48}}, 


//  down  arrow  starting  coordinates,  begins 
//  Z  velocity 


//  up  arrow  starting  coodinates,  begins  on  Z 
//  velocity 


char  s[32]; 

winset(control_win); 

pushmatrix(); 

ortho2(0.0,  769.0,  0.0,  100.0); 
RGBcolor(255, 255,255); 
clear(); 

//Go  &  Reset  Buttons 
RGBcolor(200, 200,200); 
rectfi:475.0,  25.0,  525.0,  75.0); 
rectfi:550.0,  25.0,  605.0,  75.0); 
rectf(625.0,  25.0,  675.0,  75.0); 
rectf(700.0,  25.0,  750.0,  75.0); 
RGBcolor(0,0,0); 
cmov2(482,  50); 
charstr("Rotate"); 
cmov2(487,  40); 
charstrC'Body"); 
cmov2(555,  50); 
charstr("Gravity"); 
cmov2(552,  40); 
charstr("Gradient"); 
cmov2(639,50); 
charstr("Spin"); 
cmov2(643,40); 
charstrC’up"); 
cmov2(7 10,45); 
charstr("Reset"); 
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//  Angular  Velocity 

RGBcolor(0,0,255), 

rectf(140.0,  40.0,  190.0,  70.0), 

RGBcolor(255,255,0); 

cmov2(142,  61); 

sprintf^s,  "%.4f',  (double)  x), 

charstr(s); 

cmov2(142,  51); 

sprintf(s,  "%,4f',  (double)  y), 

charstr(s), 

cmov2(142,  41); 

sprintf(s,  "%.4f',  (double)  z); 

charstr(s); 

//  External  Moment 

RGBcolor(0,0,255); 

rectf(210.0,  40.0,  260.0,  70.0); 

RGBcolor(255,255,0); 

cmov2(212,  61); 

sprintf(s,  "%.4f',  tl); 

charstr(s); 

cmov2(212,  51); 

sprintf(s,  "%.4f',  t2); 

charstr(s); 

cmov2(212,  41); 

sprintf(s,  "%.4f',  t3); 

charstr(s); 

//  Rotation  angle,  Theta 
RGBcoloitO, 0,255); 
rectf(290.0,  40.0,  320.0,  70.0); 
RGBcolor(200,200,200); 
rectf(280.0,  40.0,  290.0,  70.0); 
rectf(320.0,  40.0,  330.0,  70.0); 
//  Draw  up  and  down  arrows 
RGBcoIor(0,0,0); 
pt[0][0]  =  pt[0][0]  +  140.0; 
pt[l][0]  =  pt[l][0]  + 140.0; 
pt[2][0]  =  pt[2][0]  + 140.0; 
pt2[0][0]  =  pt2[0][0]  +  140.0; 
pt2[l][0]  =  pt2[l][0]  +  140.0; 
pt2[2][0]  =  pt2[2][0]  +  140.0; 
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polf2(3,pt),polf2(3,pt2); 
pt[0][l]  =  pt[0][l]+10.0, 
pt[l][l]  =  pt[l][l]  +  10.0; 
pt[2][l]  =  pt[2][l]+10.0, 
pt2[0][l]  =  pt2[0][l]+10  0; 
pt2[l][l]  =  pt2[l][l]+10.0; 
pt2[2][l]-pt2[2][l]  +  10.0, 
polf2(3,pt);polf2(3,pt2); 
pt[0][l]  =  pt[0][l]+  10.0; 
pt[l][l]  =  pt[l][l]+10.0, 
pt[2][l]  =  pt[2][l]+10.0; 
pt2[0][l]  =  pt2[0][l]  +  10.0; 
pt2[l][l]  =  pt2[l][l]  +  10.0; 
pt2[2][l]  =  pt2[2][l]+10.0, 
polG(3,pt); 
polf2(3,pt2); 

RGBcolor(255,255,0); 

if  (object  =  7)  //  PANS  AT 

{ 

cmov2(292,  61); 
sprintf(s,  "%.lf',  theta[0]); 
charstr(s); 
cmov2(292,  51); 
sprintf(s,  theta[l]); 

charstr(s); 
cmov2(292,  41); 
sprintf(s,  "%.lf',  theta[2]); 
charstr(s); 

} 

else  //  all  other  shapes 

{ 

cmov2(292,  61); 
sprintf(s,  "%.0f',  theta[0]); 
charstr(s); 
cmov2(292,  51); 
sprintf(s,  "%.0f',  theta[I]); 
charstr(s); 
cmov2(292,  41); 
sprintf(s,  "%.0f',  theta[2]); 
charstr(s); 

} 
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//Clock 

RGBcolor(255,0,0); 
rectfl;355  0.  55  0,  460  0,  85.0), 
RGBcolor(0,0,0), 
cmov2(365  0,71  0), 
charstr("Elapsed  Time"); 
RGBcolor(255,255,255); 
cmov2(390,  60); 
sprintf(s,  "%.lf',  elapsed), 
charstr(s), 

RGBcolor(0,0,0), 
cmov2(  10.0,90.0); 

charstrC'Gravity  Gradient  Control  Window"), 

cmov2(  10.0,75.0), 

charstr("Shape"), 


if  (object  >3) 

{ 

cmov2(82.0,75.0), 

charstrC'Inertia"), 

} 

else 

{ 

cmov2(92.0,75.0); 

charstrC'Size"); 

} 

cmov2(143.0,75.0), 

charstr("Ang  Vel"); 

RGBcolor(255,0,0); 

cmov2(  197.0,6 1.5); 

charstr("X"); 

RGBcolor(0,0,255); 

cmov2(  197.0,5 1.5); 

charstr("Y"); 

RGBcolor(0,0,0); 

cmov2(197.0,41.5); 

charstr("Z"); 

cmov2(2 15.0,75.0); 

charstr(  "Moment" ); 

cmov2(288.0,75.0); 

charstr("Theta"); 

cmov2(92.0,23.0); 

charstrC’Mass"); 


//  PANSAT  or  boom  object 


//  cube,  sphere,  or  cylinder 
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cmov2(143.0,23  0); 
charstr("Vel  Mag"), 
cmov2(2 11,0,23.0), 
charstr("Mom  Mag"); 
cmov2(280.0,23.0); 
charstrC  Altitude"); 

//  Select  between  the  different  base  objects 

RGBcolor(0,0,255); 

rectfl[10.0,  20.0,  70  0,  70.0); 

RGBcolor(255,255,0); 

cmov2(  15.0,61.5); 

charstr("Sphere"), 

cmov2(l  5.0,5 1.5); 

charstr("BIock"); 

cmov2(15.0,41.5); 

charstr("Cylinder"); 

cmov2(15.0,31.5); 

charstr("Add  Boom"); 

cmov2(15.0,21.5); 

charstrC'PANSAT"); 

switch(object) 

{ 

case  1 : 

RGBcolor(255,255,0); 
rectf(10.0,  60.0,  70.0,  70.0); 
RGBcolor(0,0,255); 
cmov2(  15.0,61.5); 
charstr("  Sphere"); 
break; 


case  2; 

RGBcolor(255,255,0); 
rectf(10.0,  50.0,  70.0,  60.0); 
RGBcolor(0,0,255); 
cmov2(l  5.0,5 1.5); 
charstr("Block"); 
break; 

case  3; 

RGBcolor(255,255,0); 
rectf(10.0,  40.0,  70.0,  50.0); 
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RGBcoIor(0,0,255). 
cmov2(  15  0,41.5), 
charstr(  "Cylinder"), 
break. 


case  4 

RGBcolor(255,255.0). 
rectftlO  O,  60  0,  70  0.  70  0), 
rectfl:iO  0.  30  0,  70  0.  40  0), 
RGBcolor(0,0,255), 
cmov2(15  0,61  5). 
charstr("Sphere"); 
cmov2(15  0,31  5). 
charstr("Add  Boom"), 
break, 


case  5: 

RGBcolor(255,255,0); 
rectf(10.0.  50.0,  70  0.  60.0), 
rectf(10.0,  30.0,  70.0,40.0), 
RGBcoIor(0,0  255); 
cmov2(  15  0,51.5); 
charstrC'Block"), 
cmov2(  15.0,3 1.5), 
charstr("Add  Boom"); 
break; 


case  6: 

RGBcolor(255,255,0); 
rectf(10.0,  40.0,  70.0,  50.0); 
rectfi:i0.0,  30.0,  70.0,40.0); 
RGBcolor(0,0,255); 
cmov2(15.0,41.5); 
charstr("Cylinder"); 
cmov2(15.0,31.5), 
charstr("Add  Boom"); 
break; 


case  7: 

RGBcolor(255,255,0), 
recto:  10.0,  20.0,  70.0,  30  0), 
RGBcolor(0,0,255); 
cmov2(15.0,21.5); 
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charstrC'PANSAT"); 

break; 


default; 

RGBcolor(255,255,0), 

rectf(10.0,  50.0,  70.0,  60.0); 

RGBcolor(0,0,255); 

cmov2(15.0,51.5); 

charstrC'Block"); 

break; 

} 

//  Object  Size 
RGBcolor(0, 0,255); 
rectf(90.0,  40.0,  120.0,  70.0); 
RGBcolor(200,200,200); 
rectft80.0,  40.0,  90.0,  70.0); 
rectf(120.0,  40.0,  130.0,  70.0); 

//  Draw  up  and  down  arrows 
RGBcoIor(0,0,0); 
pt[0][0]  =  pt[0][0]- 200.0; 
pt[l][0]  =  pt[l][0]- 200.0; 
pt[2][0]  =  pt[2]t0]- 200.0; 
pt2[0][0]  =  pt2[0][0]  -  200.0; 
pt2[l][0]  =  pt2[l][0]- 200.0; 
pt2[2][0]  =  pt2[2][0]  -  200.0; 

polf2(3,pt); 

polf2(3,pt2); 

pt[0][l]  =  pt[0][l]-  10.0; 
pt[l][l]  =  pt[l][l]-10.0; 
pt[2][l]  =  pt[2][l]-10.0; 
pt2[0][l]  =  pt2[0][l]-10.0; 
pt2[l][l]  =  pt2[l][l]-10.0; 
pt2[2][l]  =  pt2[2][l]-10.0; 
polG(3,pt);  polf2(3,pt2); 
pt[0][l]  =  pt[0][l].10.0; 
pt[l][l]  =  pt[l][l]-10.0; 
pt[2][l]  =  pt[2][l]-10.0; 
pt2[0][l]  =  pt2[0][l]-10.0; 
pt2[l][l]  =  pt2[l][l].10.0; 
pt2[2][l]  =  pt2[2][l]-10.0; 
polf2(3,pt);  polf2(3,pt2); 
RGBcolor(255,255,0); 
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if  (object  —  7)  //  Inertia  for  PANSAT 

{ 

RGBcolor(255,255,0), 

cmov2(92,  61); 

sprintf(s, "%  2f',  inertia[0]), 

charstr(s), 

cmov2(92,  51), 

sprintf(s,  "%.2f',  inertia[l]), 

charstr(s); 

cmov2(92,  41); 

sprintfljs,  "%.2f',  inertia[2]); 

charstr(s); 

} 

else  if  ((object  >3)  &&  (object  <  7))  //  Inertia  for  boom  object 

{ 

RGBcolor(255,255,0), 

cmov2(92,  61); 

sprintf(s,  inertia[0]); 

charstr(s); 

cmov2(92,  51); 

sprintf(s,  inertia[l]); 

charstr(s); 

cmov2(92,  41); 

sprintf(s,  inertia[2]); 

charstr(s); 

} 

else  //  size  for  cube,  sphere,  or  cylinder 

{ 

cmov2(92,  61); 
sprintf(s,  size[0]); 

charstr(s); 
cmov2(92,  51); 
sprintf(s,  size[l]); 

charstr(s); 
cmov2(92,  41); 
sprintf(s,  size[2]); 

charstr(s); 

} 

//Object  Mass 
RGBcolor(0, 0,255); 
rectf(90.0,  8.0,  120.0,  18.0); 


RGBcolor(200,200,200), 
rectf(80.0,  8.0,  90.0,  18.0), 
rectf(120  0,  8.0,  130.0,  18  0); 
//  Draw  up  and  down  arrows 
RGBcolor(0,0,0); 
pt[0][l]  =  pt[0][l]-32.0; 
pt[l][l]  =  pt[l][l]-32.0; 
pt[2][l]  =  pt[2][l]-32.0, 
pt2[0][ll  =  pt2[01[l]-32.0, 
pt2[l][l]  =  pt2[l][l]-32.0; 
pt2[2][l]  =  pt2[2][l].32.0, 
polf2(3,pt);  polf2(3,pt2); 
RGBcolor(255,255,0); 
ctnov2(92,  10); 
sprintf(s,  "%.0f',  mass), 
charstr(s), 

//  Angular  Velocity  Magnitude 
RGBcolor(0,0,255); 
rectf(145.0,  8.0,  185.0,  18.0); 
RGBcolor(200,200,200); 
rectf(135.0,  8.0,  145.0,  18.0); 
rectf(185.0,  8.0,  195.0,  18.0); 
//  Draw  up  and  down  arrows 
RGBcolor(0,0,0); 
pt[0][0]  =  pt[0][0]  +  55.0; 
pt[l][0]  =  pt[l][0]  +  55.0; 
pt[2][0]  =  pt[2][0]  +  55.0; 
pt2[0][0]  =  pt2[0][0]  +  65.0; 
pt2[l][0]  =  pt2[l][0]  +  65.0; 
pt2[2][0]  =  pt2[2][0]  +  65.0; 
polf2(3,pt); 
polf2(3,pt2); 
RGBcolor(255,255,0); 
cmov2(147,  9); 
sprintf(s,  "%.0e",  vel_mag); 
charstr(s); 

//  External  Moment  Magititude 
RGBcolor(0,0,255); 
rectf(215.0,  8.0,  255.0,  18.0); 
RGBcolor(200,200,200); 
rectf(205.0,  8.0,  215.0,  18.0); 
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rectf(255  0,  8.0,  265  0,  18  0), 

//  Draw  up  and  down  arrows 

RGBcolor(0,0,0), 

pt[0][0]  =  pt[0][0]  +  70.0; 

pt[l][0]  =  pt[l][0]  +  70.0; 

pt[2][0]  =  pt[2][0]  +  70.0, 

pt2[0][0]  =  pt2[0][0]  +  70.0, 

pt2[l][0]  =  pt2[l][0]  +  70.0; 

pt2[2][0]  =  pt2[2][0]  +  70.0; 

polf2(3,pt); 

poif2(3,pt2); 

RGBcolor(255,255,0); 

cmov2(2 1 7,  9); 

sprintf(s,  "%.0e",  mom  mag); 

charstr(s); 

//  Object  altitude 

RGBcolor(0,0,255); 

rectfl[285.0,  8.0,  325.0,  18.0); 

RGBcolor(200,200,200); 

rectf{275.0,  8.0,  285.0,  18.0); 

rectf(325.0,  8.0,  335.0,  18.0); 

//  Draw  up  and  down  arrows 

RGBcolor(0,0,0); 

pt[0][0]  =  pt[0][0]  +  70.0; 

pt[l][0]  =  pt[l][0]  +  70.0; 

pt[2][0]  =  pt[2][0]  +  70.0; 

pt2[0][0]  =  pt2[0][0]  +  70.0; 

pt2[l][0]  =  pt2[l][0]  +  70.0; 

pt2[2][0]  =  pt2[2][0]  +  70.0; 

polf2(3,pt); 

polf2(3,pt2); 

RGBcolor(255,255,0); 

cmov2(287,  9); 

sprintf(s,  "%.0f',  altitude); 

charstr(s); 

popmatrixO; 

swapbufifersQ; 


void  stat_controls(double  x,  double  y,  double  z,  double  am,  double  mass,  double  Ix, 

double  ly,  double  Iz) 
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winset(  mainwin) ; 
char  s[32], 
RGBcolor(0,0,0), 
RGBcolor(255,0,0), 
cinovs(25,  35,  0), 
charstr("X  velocity"); 
cmovs(37,  35,  0), 
sprintfl[s,  "%.5f',  x), 
charstr(s); 
cmovs(-50,  35,  0); 
charstr("Ixx"); 
cmovs(-45,  35,  0), 
sprintfl[s,  "%.5f',  Ix), 
charstr(s); 

RGBcolor(0,0,255); 
cmovs(25,  33,  0); 
charstr("Y  velocity"); 
cmovs(37,  33,  0); 
sprintf(s,  "%.5f’ ,  y); 
charstr(s), 
cmovs(-50,  33,  0); 
charstr("Iyy"); 
cmovs(-45,  33,  0); 
sprintf(s,  "%.5f',  ly); 
charstr(s); 
RGBcolor(0,0,0); 
cmovs(25,  31,  0); 
charstr("Z  velocity"); 
cmovs(37,  31,  0); 
sprintf(s,  "%.5f',  z); 
charstr(s); 
cmovs(-50,  31,  0); 
charstr("Izz"); 
cmovs(-45,  31,  0); 
sprint£(s,  "%.5f',  Iz); 
charstr(s); 
cmovs(25,  27,  0); 
charstrC'Ang  Momentum"); 
cmovs(39,  27,  0); 
sprintf(s,  "%.3f',  am); 
charstr(s); 
cmovs(-50,  27,  0); 


charstr("Mass"), 
cmovs(-45,  27,  0), 
sprintf(s,  "%  3f'  mass); 
charstr(s), 

} 


void  attach_eye_to(vector3D*  v,  int*  i) 

{ 

if  (eye  display  field  !=  NULL) 

{ 

*eye_display_field  =  1 , 

} 

eye  =  v; 

eye  display  field  =  i; 
*eye_display_field  =  0, 
twist  =  0; 

} 


void  attach_target_to(vector3D*  v) 

{ 

target  =  v; 
twist  =  0; 

} 


void  set_eye_to(double  x,  double  y,  double  z) 

{ 

if  (eye_display_field  !=  NULL) 

{ 

*eye_display  field  =  1; 
eyedisplayfield  =  NULL; 

} 

eye  =  new  vector3D(x,  y,  z); 
twist  =  0; 

} 


void  set_target_to(double  x,  double  y,  double  z) 

{ 

target  =  new  vector3D(x,  y,  z); 
twist  =  0; 
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} 


void  view() 

{ 

swapbufFersO; 

czclear(0xFFFF7200,getgdesc(GD_ZMAX)), 

loadmatrix(un), 

perspective(450, 1 .25,0.2, 1 0000.0); 

lookat((*eye)[0],(*eye)[  1  ],(*eye)[2],(*target)[0],(*target)[  1  ],(*target)[2], 
(int)  (twist  *  572.957795131)); 
display_this_object(lightobj), 

} 


void  view(quatemion  q,  vector3D  new_eye,  int  view_axis) 

{ 

Matrix  It  ={  10,0.0,0.0,0.0, 

0.0,  1.0,  0.0,  0.0, 

0.0,  0.0,  1.0,  0.0, 

0.0,  0.0,  0.0,  1.0}; 

matrix3x3  rotation,  axis; 
swapbufFersO; 

czclear(0xFFFF7200,getgdesc(GD_ZMAX)); 

loadinatrix(un); 

perspective(450, 1 .25,0.2, 1 0000.0); 
switch(view_axis) 

{ 

//Negative  Y  axis 
case  -2: 

axis. DCM_x_rotation(  1 . 5707963268); 
break; 

//Negative  X  axis 
case  -1; 

axis.DCM_y_rotation(-l. 5707963268); 
break; 

//Positive  X  axis 
case  1 : 

axis.DCM_y_rotation(  1 .5707963268); 
break; 
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//Positive  Y  axis 
case  2: 

axis. DCM_x_rotation(- 1.5707963268); 
break; 

//Positive  Z  axis 
case  3 : 

axis. DCM_y_rotation(3. 14159265359); 
break; 


} 


default: 

break; 


rotation.DCM_body_to_world(q); 
rotation  =  rotation  *  axis; 
new  eye  =  new_eye  *  -1; 


rt[0][0]  =  rotation[0];  rt[l][0]  =  rotation[3];  rt[2][0]  = 
rt[0][l]  =  rotation[l];  rt[l][l]  =  rotation[4];  rt[2][l]  = 
rt[0][2]  =  rotation[2];  rt[l][2]  =  rotation[5];  rtt2][2]  = 
rt[3][0]  =  0;  rt[3][l]  =  0;  rt[3][2]  =  0;  rt[3][3]  = 

multniatrix(rt); 


rt[0][0]=l;  rt[l][0]  =  0; 

rt[0][l]  =  0;  rt[l][l]=l; 

rt[0][2]  =  0;  rt[l][2]  =  0; 

rt[3][0]  =  new_eye[0]; 
rt[3][3]  =  1.0; 


rt[2][0]  =  0;  rt[3][0]  = 

rt[2][l]  =  0;  rt[3][l]  = 

rt[2][2]  =  l;  rt[3][2]  = 

rt[3][l]  =  new_eye[l]; 
multmatrix(rt); 


display_this_object(lightobj); 

} 


void  view_axis() 

{ 

display_this_object(axis); 

} 


void  rotate_view(int  angle) 

{ 


rotation[6];  rt[3][0]  =  0.0; 
rotation[7];  rt[3][l]  =  0.0, 
rotation[8];  rt[3][2]  =  0.0; 
1.0; 


0; 

0; 

0; 

rt[3][2]  =  new_eye[2]; 
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twist  =  angle; 


} 

#endif 
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APPENDIX  C.  RlGn)_BODY  CODE 


A.  HEADERFILE 

#ifhdef  RIGID_BOD  Y_H 
#define  RIGID_BODY_H 
#include  <math.h> 
^include  "vectorSD.H" 
#include  "quaternion. H" 
^include  "graphics. H" 
^include  "time.H" 

^include  "matrix3x3.H" 
#include  "constants.H" 

class  rigid_body 


double  mass; 
vector3D  *location; 
vector3D  velocity; 
vector3D  acceleration; 
vector3D  force; 
quaternion  orientation; 

vector3D  angvelocity; 

//  body  coordinates 

vector3D  ang_acceleration; 

//  body  coordinates 

vectorSD  moment; 
matrix3x3  inertia; 
vector3D  size; 
double  surface_area; 

OBJECT  *  shape; 
int  displayaxis; 
int  *display_shape; 
int  typebody; 
vector3D  holder  1; 
vector3D  hoIder2; 
quaternion  holder3; 

//  body  coordinates 

public: 

void  compute_inertiaO; 

rigidbodyO; 

rigid_body(int); 

rigid_body(char*); 

void  assign_mass(double); 

void  assign_size(double,  double,  double); 
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void  assign_size(double); 

void  assign_size(vector3D), 

void  assign_surface_area(double); 

void  assign_inertia(double,  double,  double); 

void  assign_inertia(vector3D); 

void  assign_orientation(double,  double,  double,  double); 
void  assign_orientation(quatemion); 
void  assign_shape(OBJECT*); 
void  assign_type(int); 

void  assign  holderl  (double,  double,  double); 
void  assign_holder2(double,  double,  double); 
void  assign_holder3  (double,  double,  double,  double); 
void  assign_holderl(vector3D); 
void  assign_holder2(vector3D); 
void  assign_holder3  (quaternion); 

//  Assign  values  to  the  items  using  doubles  in  world  coordinates 
void  assign_location(double,  double,  double); 
void  assign_velocity(double,  double,  double); 
void  assign_acceleration(double,  double,  double); 
void  assign_ang_velocity(double,  double,  double); 
void  assign_ang_acceleration(double,  double,  double); 
void  assign_force(double,  double,  double); 
void  assign_moment(double,  double,  double); 

//  Assign  values  to  the  items  using  vectorSD  in  world  coordinates 

void  assignJocation(vector3D); 

void  assign_velocity(vector3D); 

void  assign_acceleration(vector3D); 

void  assign_ang_veIocity(vector3D); 

void  assign_ang_acceleration(vector3D); 

void  assign_force(vector3D); 

void  assign_moment(vector3D); 

//  Assign  values  to  the  items  using  doubles  in  body  coordinates 
void  assign_velocity_bc(double,  double,  double); 
void  assign_acceleration_bc(double,  double,  double); 
void  assign_ang_veIocity_bc(double,  double,  double); 
void  assign_ang_acceleration_bc(double,  double,  double); 
void  assign_force_bc(double,  double,  double); 
void  assign_moment_bc(double,  double,  double); 

//  Assign  values  to  the  items  using  vector3D  in  body  coordinates 
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void  assign_velocity_bc(vector3D); 
void  assign_acceleration_bc(vector3D); 
void  assign_ang_velocity_bc(vector3D), 
void  assign_ang_acceleration_bc(vector3D), 
void  assign_force_bc(vector3D), 
void  assign_moment_bc(vector3D); 

//  Return  values  of  the  items  world  coordinates 

double  retum_mass(); 

vector3D  retum_inertia(); 

vector3D  retum_size(); 

vector3D  retum_location(); 

vector3D*  retum_location_ptr(); 

vector3D  retumvelocityO; 

vector3D  retum  accelerationO; 

quaternion  retumorientationQ; 

vector3D  retum_ang_velocityO; 

vector3D  retumangaccelerationQ; 

vector3D  retumforceQ; 

vector3D  retum_momentO; 

double  retum_surface_area(); 

OBJECT*  retum  shapeQ; 
int  retumtypeO; 
vectorSD  retumholderlQ; 
vectorSD  retum_holder20; 
quaternion  retum_holder30; 

//  Return  values  of  the  items  body  coordinates 

vectorSD  retum_velocity_bcO; 

vectorSD  retumaccelerationbcQ; 

vectorSD  retumangvelocitybcQ; 

vectorSD  retumangaccelerationbcQ; 

vectorSD  retumforcebcQ; 

vectorSD  retummomentbcO, 

void  displayO; 

void  update_stateO; 

void  update_state_rk40; 

double  update_state_rk45(double); 

void  addaxisQ; 

void  removeaxisQ; 

void  attach  eyeO; 

void  attachtargetO; 

void  attached_body_update(rigid_body); 
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void  zero(); 

''rigid_body() 

{ 

delete  shape; 
delete  location, 

} 

}; 

void  set_eye(double,  double,  double), 
void  set_target(double,  double,  double); 

#endif 

B.  SOURCE  FILE 

#ifhdefRIGID_BODY_C 
#define  RIGID_BODY_C 
^include  "rigid_body.H" 

void  rigid_body::conipute_inertia() 

{ 

switch(type_body) 

( 

case  1; 

inertia[0]  mass  *  ((si2e[l]  *  si2e[l])  +  (size[2]  *  size[2]))  / 12.0; 
inertia[4]  =  mass  *  ((si2e[0]  *  size[0])  +  (size[2]  *  size[2]))  / 12.0; 
inertia[8]  =  mass  *  ((si2e[0]  *  size[0])  +  (size[l]  *  size[l]))  / 12.0; 
break; 


case  2; 

inertia[0j  *  ((size[l]  *  size[l])  +  (size[2]  *  size[2]))  /  5.0; 

inertia[4]  =  mass  *  ((size[0]  *  size[0])  +  (size[2]  *  size[2]))  /  5.0; 
inertia[8]  =  mass  *  ((size[0]  *  size[0])  +  (size[l]  *  size[l]))  /  5.0; 
break; 


} 


case  3; 

inertia[0]  =  mass  ((size[l]  *  size[l])  +  (size[2]  *  size[2]))  /  4.0; 
inertia[4]  =  (mass  *  size[2]  *  size[2]  /  4.0)  + 

(mass  *  size[0]  *  size[0]  /12.0); 
inertia[8]  =  (mass  *  size[l]  *  size[l]  /  4.0)  + 

(mass  *  size[0]  *  size[0]  / 12.0); 

break; 
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void  rigid_body::assign_type(int  t) 

{ 

typebody  =  t, 

} 


rigidbody :  :rigid_body() 

{ 

location  =  new  vectorSD; 
mass  =  1000.0; 
size[0]  =1.0; 
size[l]  =  1.0; 
size[2]  =  1.0; 
surfacearea  =  1 .0; 
shape  =  NULL; 
typebody  =  0; 
displayaxis  =  0; 
display_shape  =  new  int; 
*display_shape  =  1; 

} 


rigid_body;:rigid_body(char*  ofF  file) 

{ 

location  =  new  vectorSD; 

mass  =  1000.0; 

size[0]  =1.0; 

size[l]  =  1.0; 

size[2]  =  1.0; 

surfacearea  =  1 .0; 

shape  =  re3d_object(ofF_file); 

ready_object_for_display(shape); 

typebody  =  0; 

displayaxis  =  0; 

display  shape  =  new  int; 

*display_shape  =  1; 

} 


rigid_body;:rigid_body(int  n) 
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location  =  new  vector3D, 
mass  =  1000,0; 
size[0]  =1.0; 
size[l]  =  1.0, 
size[2]  =1.0; 
surfacearea  =1.0; 
switch(n) 

{ 

case  100: 

shape  =  readobjectC'frame.off'); 

type_body=  100; 

break; 

case  200: 

shape  =  readobjectC'axis.ofT'); 

typebody  =  200; 

break; 


case  1: 

shape  =  read_object("cube.ofif’'); 
mass  =  1 000.0; 
inertia[0]  =  166.7; 
inertia[4]  =  166.7; 
inertia[8]  =  166.7; 
typebody  =  1; 
surfacearea  =  3.0; 
break; 


case  2; 

shape  =  readobjectC'sphere.ofr'); 

typebody  =  2; 

mass  =  1000.0; 

inertia[0]  =  100.0; 

inertia[4]  =  100.0; 

inertia[8]  =  100.0; 

surfacearea  =  .5; 

break; 


case  3. 

shape  =  readobjectC'cylenderofiT'); 
typebody  =  3; 
mass  =  1000.0; 
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inertia[0]  =  500.0, 
inertia[4]  =  333.3, 
inertia[8]  =  333.3, 
surfacearea  =  2.0; 
break; 

case  15; 

shape  =  read_object("fl5.ofr'); 
mass  =  1 9076; 
surfacearea  =  5.46; 
type_body=  15, 
break, 

case  23 : 

shape  =  read_object("rubber_ban.ofF'); 

mass  =  100; 

surfacearea  =01, 

typebody  =  23; 

break; 

case  3 1 ; 

shape  =  readobjectC'Il.off'); 
mass=  100; 
surface_area  =01; 
typebody  =  1; 
break; 

case  32: 

shape  =  read_object("12.off'); 

mass  =  100; 

surface_area  =01; 

typebody  =  1; 

break; 

case  33: 

shape  =  read_object("13.ofir); 
mass  =  100; 
surfacearea  =01; 
typebody  =  1; 
break; 

case  34: 

shape  =  read_object(''14.off'). 
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mass  =  100, 
surfacearea  =  01; 
typebody  =  1 , 
break; 

case  35: 

shape  =  read_object("15.off '); 
mass  =  100; 
surfacearea  =01; 
typebody  = 1; 
break; 

case  50; 

shape  =  read_object("shuttle.off'); 

typebody  =  50; 

mass  =  1570.8; 

inertia[0]  =  327.2; 

inertia[4]  =  392.7; 

inertia[8]  =  327.2; 

break; 

case  60; 

shape  =  read_object("pansat.off'); 

type_body  =  60; 

mass  =  46.14179; 

inertia[0]  =  0.8914372; 

inertia[4]  =  4.081316; 

inertia[8]  =  4.082231; 

break; 

case  71: 

shape  =  read_object("sphere_boom.oflE"); 

mass=  1000.0; 

inertia[0]  =  401.0; 

inertia[4]  =  473.0; 

inertia[8]  =  473.0; 

typebody  =  71; 

break; 

case  72; 

shape  =  readobjectC'cubeboom.off'); 
mass=  1000.0; 
inertia[0]  =  166.8; 
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inertia[4]  =  238.8, 
jnertia[8]  =  238  8, 
typebody  =  72; 
break, 

case  73 . 

shape  =  read_object("cylender_boom  off'); 

mass  =  1000.0, 

inertia[0]  =  501.0, 

inertia[4]  =  406.0; 

inertia[8]  =  406,0; 

typebody  =  73, 

break, 

case  90: 

shape  =  read_object("ground.off'); 

typebody  =  90; 

break; 

case  91: 

shape  =  read_object(''floor.ofiE’'); 

type_body  =  91; 

break; 


default; 

shape  =  NULL; 
type_body  =  0; 
break; 

} 

if  (type  body) 

ready_object_for_display(shape); 
displayaxis  =  0; 
displayshape  =  new  int; 

*display_shape  =  1; 

} 


void  rigid_body;;assign_shape(OBJECT*  o) 

{ 

shape  =  o; 

} 
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void  rigid  body  :  assign  mass(double  n) 
mass  =  n, 

} 


void  rigid_body:  :assign_surface_area(double  n) 
surfacearea  =  n, 

} 


void  rigid_body::assign_size(doubIe  x,  double  y,  double  z) 

{ 

size[0]  =  x; 
size[l]  =  y; 
size[2]  =  z; 

} 


void  rigid_body::assign_size(double  x) 

{ 

size[0]  =  x; 
size[l]  =  x; 
size[2]  =  x; 

} 


void  rigid  body.  :assign_size(vector3D  v) 

{ 

size[0]  =  v[0]; 
size[l]  =  v[l]; 
size[2]  =  v[2]; 

} 


void  rigid_body:  :assign_location(double  x,  double  y,  double  z) 

{ 

(•location)[0]  =  x; 

(*location)[l]  =  y; 

(*location)[2]  =  z; 

} 
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void  rigid  body  :assign_velocity(double  x,  double  y.  double  z) 

{ 

velocity[0]  =  x, 
velocity[  1  ]  =  y; 
velocity[2]  =  z; 

} 


void  rigid_body::assign_acceleration(double  x,  double  y,  double  z) 

{ 

acceleration[0]  =  x; 
acceleration[l]  =  y, 
acceleration[2]  =  z; 

} 


void  rigid_body  ::assign_force(double  x,  double  y,  double  z) 

{ 

force[0]  =  X, 
force[  1  ]  =  y, 
force[2]  =  z; 

} 


void  rigid_body:;assign_orientation(double  x,  double  y,  double  z,  double  w) 

{ 

orientation[0]  =  x; 
orientation[  1  ]  =  y; 
orientation[2]  =  z; 
orientation[3]  =  w; 

} 


void  rigid_body:;assign_orientation(quatemion  q) 

{ 

orientation[0]  =  q[0]; 
orientation[l]  =  q[l]; 
orientation[2]  =  q[2]; 
orientation[3]  =  q[3]; 

} 
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void  rigid  body:  assign  inertia(double  x,  double  y,  double  z) 

{ 

inertia[0]  =x, 
inertia[4]  =y; 
inertia[8]  =z; 

} 


void  rigid_body;  ;assign_inertia(vector3D  v) 

{ 

inertia[0]  =  v[0]; 
inertia[4]  =  v[lj; 
inertia[8]  =  v[2]; 

} 


void  rigid_body:  ;assign_ang_velocity(double  x,  double  y,  double  z) 

{ 

vectors  D  v(x,  y,  z); 
matrix3x3  rotation; 

rotation.DCM_world_to_body(orientation); 

V  =  rotation  *  v; 
ang_velocity[0]  =  v[0]; 
ang_velocity[l]  =  vf!]; 
ang_velocitv[2]  =  v[2]; 

} 


void  rigid_body;:assign_ang_acceleration(double  x,  double  y,  double  z) 

{ 

vectorSD  v(x,  y,  z); 
matrix3x3  rotation; 

rotation.DCM_world_to_body(orientation); 
v  =  rotation  *  v; 
ang_acceleration[0]  =  v[0], 
ang_acceleration[l]  =  v[l]; 
ang_acceleration[2]  =  v[2]; 

} 


void  rigid_body:;assign_moment(double  x,  double  y,  double  z) 

{ 

vectorSD  v(x,  y,  z); 
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matrix3x3  rotation, 

rotation. DCM_world_to_body(orientation); 
V  =  rotation  *  v, 
moment[0]  =  v[0]; 
moment[l]  =  v[l]; 
r  nent[2]  =  v[2]; 


void  rigid_body::assign_holderl  (double  x,  double  y,  double  z) 

{ 

holder!  [0]  =  x, 
holder!  [I]  =  y; 
holder!  [2]  =  z; 

} 


void  rigid_body::assign_holder2(double  x,  double  y,  double  z) 

{ 

holder2[0]  =  x; 
holder2[!]  =  y; 
holder2[2]  =  z; 

} 


void  rigid_body;:assign_holder3  (double  x,  double  y,  double  z,  double  w) 

{ 

holders  [0]  =  x; 
holder3[!]  =  y; 
holders  [2]  =  z; 
holders  [3]  =  w; 

} 


void  rigid_body;:assign_velocity_bc(double  x,  double  y,  double  z) 

{ 

vectorSD  v(x,  y,  z); 
matrixSxS  rotation; 

rotation. DCM_body_to_world(orientation), 

V  =  rotation  *  v; 
velocity[0]  =  v[0]; 
velocity[!]  =  v[!], 
velocity[2]  =  v[2]; 
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void  rigid_body::assign_acceleration_bc(double  x,  double  y,  double  z) 

{ 

vectors  D  v(x,  y,  z); 
matrix3x3  rotation, 

rotation.DCM_body_to_world(orientation); 

V  =  rotation  *  v; 
acceleration[0]  =  v[0]; 
acceIeration[l]  =  v[l], 
acceleration[2]  =  v[2]; 


void  rigid_body::assign_force_bc(doubIe  x,  double  y,  double  z) 

{ 

vectors  D  v(x,  y,  z); 
matrix3x3  rotation; 

rotation.  DCM_body_to_world(orientation); 

V  =  rotation  *  v; 
force[0]  =  v[0]; 
force[l]  =  v[l]; 
force[2]  =  v[2]; 

} 


void  rigid_body::assign_ang_velocity_bc(double  x,  double  y,  double  z) 

{ 

ang_velocity[0]  =  x; 
ang_velocity[l]  =  y; 
ang_velocity[2]  =  z; 

} 


void  rigid_body::assign_ang_acceleration_bc(double  x,  double  y,  double  z) 

{ 

ang_acceleration[0]  =  x; 
ang_acceleration[l]  =  y; 
ang_acceleration[2]  =  z; 

} 
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void  rigid_body  :;assign_moment_bc(double  x,  double  y,  double  z) 

{ 

moment[0]  =  x; 
moment[  1  ]  =  y; 
moinent[2]  =  z; 

} 


void  rigid_body::assign_location(vector3D  v) 

{ 

(*location)[0]  =  v[0], 

(*location)[l]  =  v[l]; 

(*location)[2]  =  v[2]; 

} 


void  rigid_body::assign_velocity(vector3D  v) 

{ 

velocity[0]  =  v[0]; 
velocity[l]  =  v[l]; 
velocity[2]  =  v[2]; 

} 


void  rigid_body::assign_acceleration(vector3D  v) 

{ 

acceleration[0]  =  v[0]; 
acceleration[l]  =  v[l]; 
acceleration[2]  =  v[2]; 

} 


void  rigid_body::assign_force(vector3D  v) 

( 

force[0]  =  v[0]; 
force[l]  =  v[l]; 
force[2]  =  v[2]; 

} 


void  rigid_body::assign_ang_velocity(vector3D  v) 

{ 

matrix3x3  rotation; 
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rotation  DCM_world_to_body(orientation), 

V  =  rotation  *  v, 

ang_velocity[0]  =  v[0]; 

ang_velocity[  1  ]  =  v[l]; 

ang_velocity[2]  =  v[2]; 

} 


void  rigid_body;:assign_ang_acceleration(vector3D  v) 

{ 

matrix3x3  rotation, 

rotation.DCM_world_to_body(onentation), 

V  =  rotation  *  v, 
ang_acceleration[0]  =  v[0], 
ang_acceleration[  1  ]  -  v[l]; 
ang_acceleration[2]  =  v[2]; 

} 


void  rigid_body:;assign_moment(vector3D  v) 

{ 

matrix3x3  rotation; 

rotation.DCM_world_to_body(orientation); 

V  =  rotation  *  v; 

moment[0]  =  v[0]; 

moment[l]  =  v[l]; 

moment[2]  =  v[2]; 

} 


void  rigid_body:;assign_holderI(vector3D  v) 

{ 

holder!  =  v; 

} 


void  rigid_body::assign_holder2(vector3D  v) 

{ 

holder2  =  v; 

} 


void  rigid_body::assign_holder3(quatemion  v) 
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holders  =  v. 


void  rigid_body:;assign_velocity_bc(vector3D  v) 

{ 

matrix3x3  rotation; 

rotation. DCM_body_to_world(orientation); 

V  =  rotation  *  v; 

velocity  [0]  =  v[0], 

velocity[l]  =  v[l]; 

velocity[2]  =  v[2]; 

} 


void  rigid_body::assign_acceleration_bc(vector3D  v) 

{ 

matrix3x3  rotation; 

rotation.DCM_body_to_world(orientation); 

V  =  rotation  *  v; 
acceleration[0]  =  v[0]; 
acceleration[l]  =  v[l]; 
acceleration[2]  =  v[2]; 

} 


void  rigid_body::assign_force_bc(vertor3D  v) 

{ 

matrix3x3  rotation; 

rotation.DCM_body_to_world(orientation); 

V  =  rotation  *  v; 

velocity  [0]  =  v[0]; 

velocity[l]  =  v[l]; 

veIocity[2]  =  v[2]; 


void  rigid_body::assign_ang_velocity_bc(vector3D  v) 

{ 


ang_velocity[0]  =  v[0]; 
ang_velocity[l]  =  v[l]; 
ang_velocity[2]  =  v[2]; 
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void  rigid_body::assign_ang_acceleration_bc(vector3D  v) 

{ 

ang_acceleration[0]  =  v[0], 
ang_acceleration[l]  =  v[l]; 
ang_acceleration[2]  =  v[2]; 

} 


void  rigid_body;;assign_moment_bc(vector3D  v) 

{ 

moment[0]  =  v[0], 
moment[l]  =  v[l]; 
moment[2]  =  v[2]; 

} 


int  rigid_body::retum_type() 

{ 

return  typebody; 

} 


double  rigid_body;;retum_massO 

{ 

return  mass; 

} 


vectorSD  rigid_body;:retum_inertiaO 

( 

vector3D  temp; 
temp[0]  =  inertia[0]; 
temp[l]  =  inertia[4]; 
temp[2]  =  inertia[8]; 
return  temp; 

} 


double  rigid_body::r  um  surface  areaQ 

{ 


return  surface  area, 

} 


vectors  D  rigid_body::retum_size() 

I 

return  size; 

} 


vectorSD  rigid  body;  :retum_location(') 

{ 

return  *Iocation; 

} 


vectorSD*  rigid  body;  :retum_location_ptrO 

{ 

return  location; 

} 


vectorSD  rigid_body;  ;retum_velocity() 

{ 

return  velocity; 

} 


vectorSD  rigidbody:  :retum_acceleration() 

{ 

return  acceleration; 

} 


vectorSD  rigid_body:;retum_forceO 

{ 

return  force; 

) 


quaternion  rigid  body:  :retum_orientationO 

{ 

return  orientation; 
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} 


vectors  D  rigid  body:  :retum_ang_velocity() 

{ 

vectorSD  v; 
matrix3x3  rotation; 

rotation. DCM_body_to_world(orientation), 
V  =  rotation  *  ang  velocity, 
return  v; 

} 


vectors  D  rigid  body;  ;retum_ang_acceleration() 

{ 

vectorSD  v; 
matrix3x3  rotation; 

rotation.DCM_body_to_world(orientation); 
V  =  rotation  *  ang  acceleration; 
return  v; 

} 


vectors  D  rigid_body;;  return  moment() 

{ 

vectors  D  v; 
matrixSxS  rotation; 

rotation.DCM_body_to_world(orientation); 
v  =  rotation  *  moment; 
return  v; 

} 


OBJECT*  rigid_body:;retum_shapeO 

{ 

return  shape; 

} 


vectorSD  rigid_body::retum_holderlO 

{ 

return  holder  1; 

} 
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vectors  D  rigid  body:  :retum_holder2() 

{ 

return  holder2; 

} 


quaternion  rigid  body :  :retum_holder3() 

{ 

return  holders, 

} 


vectorSD  rigid  body:  :retum_velocity_bc() 

{ 

vectorSD  v; 
matrixSxS  rotation; 

rotation.DCM_worId_to_body(orientation); 
V  =  rotation  *  velocity; 
return  v; 

} 


vectorSD  rigid_body:  :retum_acceleration_bcO 

{ 

vectorSD  v; 
matrixSxS  rotation; 

rotation .  DCM_world_to_body(orientation); 
V  =  rotation  *  acceleration; 
return  v; 

} 


vectorSD  rigid_body:;retum_force_bcO 

{ 

vectorSD  v; 
matrixSxS  rotation; 

rotation.DCM_world_to_body(orientation); 
V  =  rotation  *  force; 
return  v; 

} 
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vector3D  rigid  body: :retum_ang_velocity_bc() 

{ 

return  ang  velocity; 

} 


vectors  D  rigid  body:  :retum_ang_acceleration_bc() 

{ 

return  ang  acceleration, 

} 


vectors  D  rigid  body;  :retum_moment_bc() 

{ 

return  moment; 

} 


void  rigid_body:;update_stateO 

{ 

vectorSD  gravity(0.0,  -9.81,  0.0); 
matrixSxS  rotation; 
double  dt  =  read_deltaO; 

if(gravity_statusO) 

{ 

force  =  force  +  (gravity  *  mass); 

} 

if(air_resistance_statusO) 

{ 

double  magnitude; 

vectorSD  direction  =  velocity  *  -1.0; 

direction.normalizeO; 

magnitude  =  velocity.  magnitudeO  *  velocity.  magnitudeQ  *  0.12  * 
surfacearea; 

force  =  force  +  (direction  *  magnitude); 

} 

acceleration  =  force  /  mass; 

velocity  =  velocity  +  (acceleration  *  dt); 

♦location  =  *location  +  (velocity  *  dt)  -  (acceleration  *  (0.5  *  dt  *  dt)); 
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ang_acceleration[0]  =  moment[0]  /  inertia[0], 

ang_acceleration[  1  ]  =  moment[l]  /  inenia[4], 

ang_acceleration[2]  =  moment[2]  /  inertia[8]; 

ang  velocity  =  ang  velocity  +  (ang  acceleration  *  dt), 

orientation.  update(  ang  velocity ,  dt ), 

orientation.  normalizeO; 

force[0]  =  0.0, 

force[l]  =  0.0, 

force[2]  =  0.0; 

moment[0]  =  0.0; 

moment[l]  =  0.0; 

moment[2]  =  0.0; 


void  rigid  body;  :update_state_rk4() 

{ 

double  dt  =  read  deltaQ; 

double  hh  =  dt  *  .5,  h6  =  dt  /  6; 

vector3D  ya  =  ang  velocity,  dyma,  dyta,  yta,  dydxa; 

vectorSD  yv  =  velocity,  dymv,  dytv,  ytv,  dydxv; 

vectorSD  yl  =  *location,  dyml,  dytl,  ytl,  dydxl; 

quaternion  y  =  orientation,  dym,  dyt,  yt,  dydx;  int  i; 

vectorSD  gravity(0.0,  -9.81,  0.0); 

matrix3x3  rotation; 

if(gravity_status0) 

{ 

force  =  force  +  (gravity  *  mass); 

} 

if(air_resistance_statusO) 

{ 

double  magnitude; 

vector3D  direction  =  velocity  *  -1.0; 

direction.normalizeO; 

magnitude  =  velocity.  magnitudeQ  *  velocity.  magnitudeO  *  0.12  * 
surfacearea; 

force  =  force  +  (direction  *  magnitude); 

} 

acceleration  =  force  /  mass; 
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dydxv  =  acceleration,  dydxl  =  velocity, 

dydxa[0]  =  (moment[0]  -  (ang_velocity[  1  ]  *  ang_velocity[2]  *  (inertia[8]  - 
inertia[4])))  /  inertia[0], 

dydxa[l]  =  (moment[l]  -  (ang_velocity[0]  *  ang_velocity[2]  *  (inenia[0]  - 
inertia[8])))  /  inertia[4]; 

dydxa[2]  =  (moment[2]  -  (ang_velocity[  1  ]  *  ang_velocity[0]  *  (inertia[4]  - 
inertia[0])))  /  inertia[8], 

dydx[0]  =  -0.5*((orientation[l]  *  ang_velocity[0])  +  (orientation[2]  * 
ang_velocity[  1  ])  +  (orientation[3]  *  ang_velocity[2])); 
dydx[l]  =  0.5*((orientation[0]  ♦  ang_velocity[0])  +  (orientation[2]  * 
ang_velocity[2])  -  (orientation[3]  *  ang_velocity[l])); 
dydx[2]  -  0.5*((orientation[0]  *  ang_velocity[l])  +  (orientation[3]  * 
ang_velocity[0])  -  (orientation[l]  *  ang_velocity[2])), 
dydx[3]  =  0  5*((orientation[0]  *  ang_velocity[2])  +  (orientation[  1  ]  * 
ang_velocity[l])  -  (orientation[2]  *  ang_velocity[0])); 


for(i  =  0;  i  <  3;  i-H-) 

( 

yt[i]  =  y[i]  +  hh  *  dydx[i]; 
yta[i]  =  ya[i]  +  hh  *  dydxa[i]; 
ytv[i]  =  yv[i]  +  hh  *  dydxv[i]; 
ytl[i]  =  yl[i]  +  hh  •  dydxl[i]; 

} 

yt[51  =  y[31  +  hh  *  dydx[3]; 
dytv  =  acceleration;  dytl  =  ytv; 

dyt[0]  =  -0.5*((yt[l]  *  yta[0])  +  (yt[2]  *  yta[l])  +  (yt[3]  *  yta[2])); 

dyt[l]  =  0.5*((yt[0]  *  yta[0])  +  (yt[2]  *  yta[2])  -  (yt[3]  *  yta[l])); 

dyt[2]  =  0.5*((yt[0]  *  yta[l])  +  (yt[3]  *  yta[0])  -  (yt[l]  *  yta[2])); 

dyt[3]  =  0.5*((yt[0]  *  yta[2])  +  (yt[l]  *  yta[l])  -  (yt[2]  *  yta[0])); 

dyta[0]  =  (moment[0]  -  (yta[l]  *  yta[2]  *  (inertia[8]  -  inertia[4])))  /  inertia[0]; 

dyta[l]  =  (moment[l]  -  (yta[0]  *  yta[2]  *  (inertia[0]  -  inertia[8])))  /  inertia[4]; 

dyta[2]  =  (monient[2]  -  (yta[l]  *  yta[0]  *  (inertia[4]  -  inertia[0])))  /  inertia[8]; 


for(i  =  0;  i  <  3;  i-H-) 

{ 

yt[i]  =  y[i]  -t-  hh  *  dyt[i]; 
yta[i]  =  ya[i]  -•-  hh  *  dyta[i]; 
ytv[i]  =  yv[i]  -t-  hh  *  dytv[i]; 
ytl[i]  =  yl[i]  +  hh  *  dytl[i], 

} 

yt[3]  =  y[3]-Hhh*dyt[3]; 
dymv  =  acceleration; 
dyml  =  ytv; 
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dym[0]  =  -0.5*((yt[l]  ♦  yta[0])  +  ^yt[2]  *  yta[l])  -  (vt[3]  *  yta[2])); 
dym[l]  =  0.5*((yt[0]  *  yta[0])  +  (yt[2]  *  vta[2])  -  (yt[3]  *  yta[l])), 

dym[2]  =  0  5*((yt[0]  *  yta[l])  +  (yt[3]  *  yta[0])  -  (yt[l]  *  yta[2])); 

dyni[3]  =  0  5*((yt[0]  *  yta[2])  +  (yt[l]  *  yta[l])  -  (yt[2]  *  yta[0])), 

dyma[0]  ==  (moment[0]  -  (yta[l]  *  yta[2]  *  (inertia[8]  -  inertia[4])))  / 

inertia[0], 

dyma[l]  =  (moment[l]  -  (yta[0]  *  yta[2]  *  (inertia[0]  -  inertia[8])))  / 
inertia[4]; 

dyma[2]  =  (moinent[2]  -  (>ta[l]  *  yta[0]  *  (inertia[4]  -  inertia[0])))  / 
inenia[8], 


for(i  =  0;  i  <  3;  1++) 

{ 

yt[i]  =  y[i]  +  dt  *  dym[i], 
yta[i]  =  ya[i]  +  dt  *  dyma[i]; 
ytv[i]  =  yv[i]  +  dt  *  dyinv[i]; 
ytl[i]  ==  yl[i]  +  dt  *  dyml[i]; 

} 

yt[3]  =  y[3]  +  dt  *  dyin[3]; 

for(i  =  0,  i  <  3;  i+-i-) 

{ 

dym[i]  =  dym[i]  +  dyt[i]. 
dyma[i]  =  dyma[i]  +  dyta[i]; 
dymv[i]  =  dymv[i]  +  dytv[i]; 
dyml[i]  =  dyml[i]  +  dytl[i]; 

} 

dym[3]  =  dym[3]  +  dyt[3]; 
dytv  =  acceleration; 
dytl  =  ytv, 

dyt[0]  =  -0.5*((yt[l]  *  yta[0])  +  (yt[2]  *  >'ta[l])  +  (yt[3J  *  yta[2])); 

dyt[l]  =  0,5*((yt[0]  *  yta[0])  +  (yt[2]  *  yta[2])  -  (yt[3]  *  yta[l])); 

dyt[2]  =  0.5*((yt[0]  *  yta[l])  +  (yt[3]  *  yta[0])  -  (yt[l]  *  yta[2])); 

dyt[3]  =  0.5*((yt[0]  *  yta[2])  +  (yt[l]  *  ytall])  -  (yt[2]  *  yta[0])); 

dyta[0]  =  (moment[0]  -  (yta[l]  *  yta[2]  *  (inertia[8]  -  inertia[4])))  /  inertia[0]; 

dyta[l]  =  (moment[l]  -  (yta[0]  *  yta[2]  *  (inertia[0]  -  inertia[8])))  /  inertia[4]; 

dyta[2]  =  (moment[2]  -  (yta[l]  ♦  yta[0]  *  (inertia[4]  -  inertiafo])))  /  inertia[8]; 


for(i  =  0;  i  <  3;  i-H-) 

{ 

orientation[i]  =  y[i]  +  h6  *  (dydx[i]  +  dyt[i]  +  2.0  *  dym[i]); 
ang_velocity[i]  =  ya[i]  +  h6  ♦  (dydxa[i]  +  dyta[i]  +  2.0  *  dyma[i]); 
(*location)[i]  =  yl[i]  +  h6  *  (dydxl[i]  +  dytl[i]  +  2.0  *  dyml[i]); 
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velocity[i]  =  yv[i]  +  h6  *  (dydxv[i]  +  dytv[i]  +  2.0  *  dyTnv[i]); 

orientation[3]  =  y[3]  +  h6  *  (dydx[3]  +  clyt[3]  +  2.0  *  dym[3]), 

orientation.  normalize(); 

force[0]  =  0.0; 

force[l]  =  0.0; 

force[2]  =  0.0; 

moment[0]  =  0.0; 

moment[l]  =  0.0; 

moment[2]  =  0.0, 


double  rigid_body:;update_state_rk45(double  eps) 

{ 

double  PRGOW  =  -0.20,  PSHRNK  =  -0.25,  FCOR  =  .06666666, 
dt  =  readdeltaO; 

double  SAFETY  =  0.9,  ERRCON  =  6.0e-4,  xsav  =  dt,  htry  =  dt; 
double  P  =  ang_velocity[0],  Q  =  ang_velocity[l],  R  =  ang_velocity[2]; 
double  hh  =  dt  *  .5,  h6  =  dt  /  6,  h,  dtl,  hhl,  h61,  errmax; 
vector3D  ya  =  ang  velocity,  dyma,  dyta,  yta,  dydxa,  dysava,  ysava,  ytempa, 
ytemp2a; 

vectorSD  yv  =  velocity,  dymv,  dytv,  ytv,  dydxv,  dysaw,  ysaw,  ytempv,  ytemp2v; 
vectorSD  yl  =  *location,  dyml,  dytl,  dydxl,  dysavl,  ysavl,  ytempl,  ytemp21; 
quaternion  y  =  orientation,  dym,  dyt,  yt,  dydx,  dysav,  ysav,  ytemp,  ytenip2;  int  i; 
vector3D  gravity(0.0,  -9.81,  0.0); 
matrix3x3  rotation; 

if(gravity_status0) 

{ 

force  =  force  +  (gravity  *  mass); 

} 

if(air_resistance_statusO) 

{ 

double  magnitude; 

vectorSD  direction  =  velocity  *  -1.0; 

direction.normalizeO; 

magnitude  =  velocity. magmtudeO  *  velocity.magnitudeQ  *  0.12  • 
surfacearea; 

force  =  force  +  (direction  *  magnitude); 

} 

acceleration  =  force  /  mass; 
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for(i  =  0,  i  <  3,  i-*-+) 


ysav[i]  =  y[i], 
dysav[i]  =  dydx[i], 
ysava[i]  =  ya[i], 
dysava[i]  =  dydxa[i]. 
ysaw[i]  ~  yv[i], 
dysaw[i]  =  dydxv(']- 
ysavl[i]  =  yl[i]. 
dysavl[i)  =  dydxlfi], 

[ 

ysav[3]  =  y(3], 
dysav(3]  =  dydx(3J. 
h  =  htrv’, 
for(,.) 

j 

hh  =  0  5  •  h. 
dtl  =hh, 
hhl  =dtl  *0  5, 
h61  =dtl  /6  0, 
dydxv  =  acceleration; 
dydxl  =  velocity; 

dydxa[0]  =  (moment[0]  -  (ang_velocity[l]  *  ang_velocity[2]  *  (inertia[8] 
inertia[4])))  /  inertia[0]; 

dydxa[l]  =  (monient[l]  -  (ang_velocity[0]  *  ang_velocity[2]  *  (inertia[0] 
inertia[8])))  /  inertia(4]; 

dydxa[2]  =  (moment[2]  -  (ang_velocity[l]  *  ang_velocity[0]  *  (inertia[4] 
inertia[0])))  /  inertia[8]; 

dydx[0]  =  -0.5*((orientation[l]  *  ang_velocity[0])  +  (orientation[2]  * 
ang_velocity[l])  +  (orientation[3]  *  ang_velocity[2])); 
dydx[l]  =  0.5*((orientation[0]  *  ang_velocity[0])  +  (orientation[2]  * 
ang_velocity[2])  -  (orientation[3]  *  ang_velocity[l])); 
dydx[2]  =  0.5*((orientation[0]  *  ang_velocity[l])  +  (orientation[3]  * 
ang_velocity[0])  -  (orientation[l]  *  ang_velocity[2])); 
dydx[3]  =  0.5*((orientation[0]  *  ang_velocity[2])  +  (orientation[l]  * 
ang_velocity[l])  -  (orientation[2]  *  ang_velocity[0])); 


for(i  =  0;  i  <  3;  i++) 

{ 

yt[i]  =  ysav[i]  +  hhl  *  dydx[i]; 
yta[i]  =  ysava[i]  +  hhl  •  dydxa[i], 
ytv[i]  =  ysaw[i]  +  hhl  *  dydxv[i]; 
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ytl[i]  =  ysavlfi]  +  hhl  *  dydxl[i]. 

! 

>1(3]  =  ysav{3]  ^  hhl  *  dydx{3]. 
dvi\  =  acceleration, 
dvtl  ^  vtv. 

d>l(0]  =  -0  5*((yt(l]  •  ytafOj)  ^  (ytI2]  •  yiaflj)  -  (yi(3]  •  yTa(2])). 
d\i(  1 1  =  0  5*((  /t(0]  *  vta{0])  -  (yt(2]  •  yta{2])  -  (yt[3]  *  \ia[  1  ])). 

dM(2]  -  0  s*((M|0]  •  yiaflj)  -  (yt(3]  *  via[0])  -  (>i(  1  ]  *  yia(2|)). 

d\i(3]  -  0  5*((M(0]  •  \ia{2])  -  (vi(l|  •  via(I])-(vi[2]  *  ylafO])). 

d\iajO]  -  (moment(O)  -  (viallj  *  via|2]  *  |incrtia{8]  -  inertia(4]))) 

inenia(0|. 

d\ia(  1  ]  -  (momenil  1  ]  -  (via(0]  *  \ia(2]  *  (inertiafO)  -  inenia(8]))) 

inenia|4j. 

d\ia|2]  -  (moment{2]  -  (yta(  1 1  *  vla(0]  *  (menia[4]  -  inenia(0]))) 

mcrtia{8]. 


fort  I  =  0.  I  '  3  .1^'^) 

I 

I 

yl(i]  =  ysav|i]  hhl  *  dyt(i], 
yta(i]  =  ysavafi]  hh!  *  dyta(i]. 
ytv(i]  =  ysaw[il  +  hhl  *  dytv[i], 
ytl(i]  ^  ysavifi]  +  hhl  *  dytifi], 

1 

yt(3]  =  ysav[3]  +  hhl  *  dyt(3J, 
dymv  =  acceleration, 
dyml  =  ytv, 

dym(0]  =  -0  5*((yt(l]  *  yta(0])  +  (yt[2]  *  yta[l])  +  (yt[3]  •  yta[2])); 
dym[l]  =  0  5*((yt[0]  *  yta[0])  +  (yt[2]  *  yta[2])  -  (yt[3]  *  yta[l])); 
dyni[2]  =  0,5*((yt(0]  *  yta[l])  +  (yt[3]  *  yta[0])  -  (yt[l]  *  yta[2])), 
dym[3]  =  0.5*((yt[0]  *  yta[21)  +  (yt[l]*  yta[l])  -  (yt[2]  *  yta[0])); 
dynia[0]  =  (moment[0]  -  (^[1]  *  yta[2]  *  (inertia[8]  -  inertia[4])))  / 
inertia(0]; 

dyma[l]  =  (moment[l]  -  (yta[0]  ♦  yta[2]  *  (inertia[0]  -  inertia[8])))  / 
inertia[4]; 

dyma[2]  =  (moment[2]  -  (yta[l]  ♦  yta[0]  *  (inertia[4]  -  inertia[0])))  / 
inertia[8]; 


for(i  =  0;  i  <  3;  1++) 

{ 

yt[i]  =  ysav[i]  +  dtl  *  dym[i]; 
yta[i]  =  ysava[i]  +  dtl  •  dyma[i]; 
ytv[i]  =  ysaw[i]  +  dtl  *  dyntv[i]; 
ytl[i]  =  ysavl[i]  +  dtl  *  dyrnl[i]; 
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I 

vl[3]  =  ’  >  dll  *  dym(3J, 

for(i  '  . 

I 

dyni[i]  =  dyni[i]  +  dyt[i], 
dymafi]  =  dymafi]  +  dyta(i], 
dyinv(i)  =  dymvfi]  +  dytv[i], 
dymJfi]  =  dyml[i]  ♦  dytlfi]. 

[ 

dym{3]  =  dym(3] dyt(31, 
dM\  =  acceleration. 
dvll  =  ytv. 

d>l(0|  =  -0  5*((yt(l]  •  yta(0]'  f2]  •  yta[l])  +  (vt(3)  *  vta[2])). 

dyt(l)-0  5*((yt{0]*yta(0:  ^  *  yta[2))  -  (yt[3]  *  yta(l])). 

dyt(2]  =  0  5*((yt[0]  *  yta(ll)  M  .  •]  *  yu(0])  -  (yt[l]  •  yta(2])). 

dyt[3]  =  0  5*((yt[0]  *  yta[2l)  +  (ytl I]  ‘  yta[I])  -  (yt[2]  *  yta[0))). 

dytafO]  =  (moment[0]  -  (yta(l]  *  yta[2]  •  (inertia[8]  -  inenia[4])))  / 

inertiafO], 

dyta(  I  ]  =  (moment[  I  ]  -  (yta{0]  *  yta[2]  *  (inert  :a[0]  -  inertit.[8])))  / 

inertia[4], 

dyta[2]  =  (moment[2]  -  (yta[lj  *  yu[0]  •  (incrtia[4]  -  inertia[Oj)/i  / 

inertia[8]. 


for(i  =  0,  i  <  3,  i-M-) 

{ 

yteinp[i]  =  ysav[i]  +  h6l  *  (dydx[i]  +  dyt[i]  +  2  0  *  dyin[i]); 
ytenipa[i]  =  ysava[i]  +  h6l  *  (dydxa[i]  +  dyta[i]  +  2.0  *  dyma[i]); 
ytempl[i]  =  ysavl[i]  +  h6l  *  (dydxl[i]  +  dytl[i]  +  2.0  *  dynil[i]); 
ytempv[i]  =  ysaw[i]  +  h61  *  (dyd;w[i]  +  dytv[i]  +  2.0  *  dymv[i]), 

} 

ytemp[3]  =  ysav[3]  +  h61  *  (dydx[3]  +  dyt[3]  +  2.0  *  dyni[3]); 

//assign  new  y's  for  second  rk4 
for(i  =  0;  i  <  3;  i++) 

{ 

y[i]  =  ytemp[i]; 
ya[i]  =  ytempa[i]; 
yl[i]  =  ytempl[i]; 
yv[i]  =  yteinpv[i]; 

} 

y[3]  =  ytemp[3]; 
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//calculate  new  set  of  derivatives 

dydxv  =  acceleration; 

dydx^i  =  ytempv; 

dydx[0]  =  ~0  5*((ytemp[l]  *  ytempa[0])  +  (ytemp[2]  *  ytempa[l])  + 
(ytemp[3]  *  ytempa[2])); 

dydx[l]  =  0.5*((ytemp[0]  *  ytempa[0])  +  (ytemp[2]  *  ytempa[2])  - 

(ytemp[3]  *  ytempa[  1  ])); 

dydx[2]  =  0.5*((ytemp[0]  *  ytempa[l])  +  (ytemp[3]  *  ytempa[0])  - 

(ytemp[l]  *  ytempa[2])); 

dydx[3]  =  0.5*((ytemp[0]  *  ytempa[2])  +  (ytemp[l]  *  ytempa[l])  - 

(ytemp[2]  *  ytempa[0])); 

dydxa[0]  =  (moment[0]  -  (ytempa[l]  *  ytempa[2]  *  (inertia[8]  - 
inertia[4])))  /  inertia[0]; 

dydxafl]  =  (moment[l]  -  (ytempa[0]  *  ytempa[2]  *  (inertia[0]  - 
inertia[8])))  /  inertia[4]; 

dydxa[2]  =  (moment[2]  -  (yteinpa[l]  *  ytempa[0]  *  (inertia[4]  - 
inertia[0])))  /  inertia[8]. 


for(i  =  0,  i  <  3,  i-t-+) 

{ 

yt[']  =  y[i]  hhl  *  dydx[i]. 
yta[i]  =  ya[i]  +  hhl  *  dydxa[i], 
ytv[i]  =  yv[i]  +  hhl  *  dydxv[i]. 
ytl[i]  =  yl[i]  +  hhl  *  dydxJfi], 

} 

yt[3]  =  y[3]  +  hhl  *  dydx[3]. 
dytv  =  acceleration, 
dytl  =  ytv; 

dyt[0]  =  -0.5*((yt[l]  *  yta[0))  +  (yt[2]  *  yta[l])  +  (yt[3]  *  yta[2])), 
dyt[l]  =  0.5*((yt[0]  •  yta[01)  +  (ytl2]  *  yta[2])  -  (yt[3]  *  yta[l])), 

dyt[2]  =  0  5*((yt[0]  *  yta[ll)  +  (yt[3]  *  yta[0])  -  (yt[l]  *  yta[2])), 

dyt[3]  =  0  5*((yt[0]  *  yte[2])  +  (yt[\]  *  yta[l])  -  (yt[2]  *  yta[0])>; 

dyta[0]  =  (moment[0]  -  (yta[l]  *  yta[21  *  (inertia[8]  -  inertiri4j)))  / 

inertia[0]; 

dyta[l]  =  (monient[l]  -  (yta[0]  *  yta[2]  *  (inertia[0]  -  inertia[8])))  / 

inertia[4]; 

dyta[2]  =  (itioment[2]  -  (yta[l]  *  yta[0]  *  (inertiar4]  -  inertia[0])))  / 

inertia[8]; 


for(i  =  0;  i  <  3;  \++) 

{ 

yt[i]  =  y[i]  +  hhl  •  dyt[i]; 
ytali]  =  ya[i]  +  hhl  *  dyta[i]. 
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ytv[i]  =  yv[i]  +  hhl  *  dytv[i]; 
ytl[i]  =  yl[i]  +  hhl  *  dvtl[i], 

} 

yt[3]  =  y[3]  --hhl  *  dyt[3], 
dymv  =  acceleration; 
dyml  =  ytv; 

dym[0]  =  -0.5*((yt[l]  *  yta[0])  +  (yt[2]  *  yta[l])  +  (yt[3]  *  yta[2])); 
dym[l]  =  0.5*((yt[0]  *  yta[0])  +  (yt[2]  *  yta[2])  -  (yt[3]  *  yta[l])); 

dym[2]  =  0.5*((yt[0]  *  yta[I])  +  (yt[3]  *  yta[0])  -  (yt[l]  *  yta[2])); 

dym[3]  =  0.5*((yt[0]  *  yta[2])  +  (yt[;]  *  yta[l])  -  (yt[2]  *  yta[0])); 

dyma[0]  =  (moment[0]  -  (yta[l]  *  yta[2]  *  (inertia[8]  -  inertia[4])))  / 

inertia[0], 

dynia[l]  =  (moment[l]  -  (yta[0]  *  yta[2]  *  (inertia[0]  -  inertia[8])))  / 
inertia[4], 

dyma[2]  =  (moment[2]  -  (yta[l]  *  yta[0]  *  (inertia[4]  -  inertia[0])))  / 
mertia[8]. 


for(i  =  0,  i  <  3.  i-t-+) 


yt[i]  =  y[ij  +  dtl  *  dym[i]. 
yta[i]  =  ya(i]  +  dtl  *  dyma[i]. 
ytv[i]  =  yv[i]  +  dtl  *  dyinv[i], 
ytl[ij  =  yl[i]  +  dtl  •  dy^[i], 

} 

yt[31  =  yl3]  +  dtl  •  dym[3]. 


for(i  =  0,  i  <  3, 1++) 

{ 

dym[i]  =  dyni[i]  +  dyt[i]; 
dyma[i]  =  dyina[i]  +  dyta[i]; 
dymv[i]  =  dymv[i]  +  dytv[i], 
dyml[i]  =  d^[i]  +  dytl[i]; 

) 

dyin[3]  =  dyfn[3]  +  dyt[3], 
dytv  =  acceleration; 
dytl  =  ytv, 

dyt[0]  =  -0.5*((yt[l]  *  yto[0J)  +  (yt[2]  *  yta[l])  +  (yt[3]  *  yta[2])); 
dyt[ll  =  0.5*((yt[0]  *  yta[01)  +  (yt[2]  *  yta(2])  -  (yt[3]  *  yta[l])), 

dyt[2]  =  0.5*((yt[0]  •  yta[ll)  +  (yt[3]  *  yta[0])  -  (yt[l]  *  yta[2])), 

dyt[3]  =  0.5*((yt[0]  *  yta(2])  +  (yt[l]  *  yta[l])  -  (yt[2]  *  yta[0])), 

dyta[0]  =  (nioment[0]  -  (yta[l]  *  yta[2]  *  (inertia[8]  -  inertia[4])))  / 

inertia[0]. 
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dyta[l]  =  (moment[l]  -  (yta[0]  *  yta[2]  *  (inertia[0]  -  inertia[8])))  / 

inertia[4]; 

dyta[2]  =  (moment[2]  -  (yta[l]  *  yta[0]  *  (inertia[4]  -  inertia[0])))  / 

inertia[8]; 


for(i  =  0;  i  <  3;  i-t-+) 

{ 

ytemp2[i]  =  y[i]  +  h61  *  (dydx[i]  +  dyt[i]  +  2.0  *  dym[i]); 
ylemp2a[i]  =  ya[i]  +  h61  •  (dydxa[i]  +  dyta[i]  +  2.0  *  dyma[i]); 
ytemp21[i]  =  yl[i]  +  h61  *  (dydxl[i]  +  dytl[i]  +  2.0  *  dyml[i]); 
ytemp2v[i]  =  yv[i]  +  h61  *  (dydxv[i]  +  dytv[i]  +  2.0  *  dymv[i]); 

} 

ytenip2[3]  =  y[3]  +  h61  *  (dydx[3]  +  dyt[3]  +  2  0  *  dyin[3]); 

//third  rk4  run 
dtl  =  h; 

hhl  =dtl  *0.5; 
h51  =  dtl  /  6.0; 
dydxv  =  acceleration, 
dydxl  =  velocity; 

dydxa[0]  =  (nioment[0]  -  (ang_velocity[  1  ]  *  ang_veIocity[2]  *  (inertia[8] 

inertia[4])))  /  inertia(0]; 

dydxa[l]  =  (moment[l]  -  (ang_velocity(0]  *  ang_velocity[2]  *  (inertia[0] 

inertia[8])))  /  mertia[4]; 

dydxa[2]  =  (moment[2]  -  (ang_velocity[l]  *  ang  velocityfO]  *  (inertia[4] 

inertia[0])))  /  inertia[8], 

dydx[0]  =  -0.5*((orientation[ll  *  ang_velocity[0])  +  (orientation[2]  * 
ang_velocity{l])  +  (orientation[33  *  ang_velocity[2])), 
dydx[l]  =  0  5*((orientation[0J  *  ang_velocity[0])  +  (orientation[2]  * 
ang_velocity(2])  -  (orientation[3]  *  ang_velocity[l])); 
dydx[2]  =  0.5*((orientation[0]  *  ang_velocity[  1  ])  +  (orientation[3]  * 
ang_velocity[0])  -  (orientation[  1  ]  *  ang_velocity[2])); 
dydx[3]  =  0.5*((orientation[0]  *  ang_velocity[2])  +  (orientation[  1  ]  * 
ang_velocity[ll)  -  (orientation[2]  ♦  ang_velocity[0])); 


for(i  =  0;  i  <  3,  i-H-) 

{ 

yt[i]  =  ysav[i]  +  hhl  *  dydx[i]; 
yta[i]  =  ysava[i]  +  hhl  *  dydxa[i]; 
ytv[i]  =  ysaw[i]  +  hhl  *  dydxv[i]; 
ytl[i]  =  ysavl[i]  +  hhl  *  dydxl[i]; 

} 

yt[3]  =  ysav[3]  +  hhl  *  dydx[3]; 
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dytv  =  acceleration, 
dytl  =  ytv, 

dyt[0]  =  -0  5*((yt[l]  *  vta[0])  +  (yt[2]  *  yta[l])  +  (yt[3]  *  yta[2])). 
dyt[l]  =  0  5*((yt[0]  *  yta[01)  +  (yt[2]  *  yta[2])  -  (yt[3]  *  yta[l])); 

dyt[2]  =  0,5*((yt[0]  *  yta[l])  +  (yt[3]  *  yta[0])  -  (yt[l]  *  yta[2])); 

dyt[3]  =  0.5*((yt[0]  *  yta[2])  +  (yt[l]  *  yta[l])  -  (yt[21  *  yta[0])); 

dyta[0]  =  (moment[0]  -  (yta[l]  *  yta[2]  *  (inertia[8]  -  inertia[4])))  / 

inertia[0]; 

dyta[l]  =  (moment[l]  -  (yta[0]  *  yta[2]  *  (inertia[0]  -  inertia[8])))  / 

inertia[4], 

dyta[2]  =  (moment[2]  -  (yta[l]  *  yta[0]  *  (inertia[4]  -  inertia[0])))  / 

inertia[8]; 


for(i  =  0;  i  <  3;  i-H-) 

{ 

yt[i]  =  ysav[i]  +  hhl  *  dyt[i]; 
yia[i]  =  ysava[i]  +  hhl  *  dyta[i]; 
ytv[i]  =  ysaw[i]  +  hhl  *  dytv[i], 
ytl[i]  =  ysavl[i]  +  hhl  *  dytl[i]; 

} 

yt[3]  =  ysav[3]  +  hhl  *  dyt[3], 
dymv  =  acceleration, 
dyml  =  ytv; 

dym[0]  =  -0  5*((yt[l]  *  yta(Ol)  +  (yt[2]  *  yta[I])  +  (yt[3]  *  yta[2])), 
dyni[l]  =  0  5*((yt[0]  *  yta[0])  +  (yt[2]  *  yta[2])  -  (yt[3]  *  yta[l])); 

dyin[2]  =  0  5*((yt[0]  *  yta[l])  +  (yt[3]  •  yta[0])  -  (yt[l]  *  r^a[2])); 

dym[3]  =  0  5*((yt[0]  *  yta[2])  +  (yt[l]  *  yta[l])  -  (yt[2]  *  yta[0])), 

dyma[0]  =  (moment[0]  -  (yta(l]  *  yta[2]  *  (inertia[8]  -  inertia[4])))  / 

inertia[0], 

dyma[l]  =  (inoment[l]  -  (yta[0]  *  yta(2]  *  (inertia[0]  -  inertia[8])))  / 
inertia[4], 

dyma[2]  =  (moment[2]  -  (yta[l]  *  yta[0]  *  (inertia[4]  -  inertia[0])))  / 
inertia[8]. 


for(i  =  0,  i  <  3,  ’i++) 

{ 

yt[i]  =  ysav[i]  +  dtl  *  dym[i], 
yta[i]  =  ysava[i]  +  dtl  *  dyma[i], 
ytv[i]  =  ysaw[i]  +  dtl  *  dymv[i], 
ytl[i]  =  ysavl[i]  +  dtl  *  dyiTil[i], 

} 

yt[3]  =  ysav[3]  +  dtl  *  dym[3]; 
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for(i  =  0,  i  <  3,  i-H-) 

t 

dym[i]  =  dyin[i]  +  dyt[i]; 
dyma[i]  =  dyma[i]  +  dyta[i], 
dyniv[i]  =  dymv[i]  +  dytv[i]; 
dyml[i]  =  dyml[i]  +  dytl[i], 

} 

dyni[3]  =  dym[3]  +  dyt[3]; 
dytv  =  acceleration; 
dytl  =  ytv; 

dyt[0]  =  -0.5*((yt[l]  *  yta[0])  +  (yt[2]  *  yta[l])  +  (yt[3]  *  yta[2])); 
dyt[l]  =  0.5*((yt[0]  *  yta[0])  +  (yt[2]  *  yta[2])  -  (yt[3]  *  yta[l])); 

dyt[2]  =  0.5*((yt[0]  *  yta[l])  +  (yt[3]  *  yta[0])  -  (yt[l]  *  yta[2])); 

dyt[3]  =  0.5*((yt[0]  *  yta[2])  +  (yt[l]  *  yta[l])  -  (>t[2]  *  ytafo])); 

dyta[0]  =  (moment[0]  -  (yta[l]  *  yta[2]  *  (inertia[8]  -  inertia[4])))  / 

inertia[0]; 

dyta[l]  =  (nioment[l]  -  (yta[0]  *  yta[2]  *  (inertia[0]  -  inertia[8])))  / 

inertia[4]; 

dyta[2]  =  (moinent[2]  -  (yta[l]  *  yta[0]  *  (inertia[4]  -  inertia[0])))  / 

inert  ia[8]; 


for(i  =  0;  i  <  3;  i-H-) 

( 

ytemp[i]  =  ysav[i]  +  h61  *  (dydx[i]  +  dyt[i]  +  2.0  ♦  dym[i]); 
ytenipa[i]  =  ysava[i]  h61  *  (dydxa[i]  -+-  dyta[i]  +  2.0  *  dyma[i]); 
ytempl[i]  =  ysavl[i]  +  h61  *  (dydxl[j]  +  dytl[i]  +  2.0  *  dyinl[i]); 
ytenipv[i]  =  ysaw[i]  h-  h61  *  (dydxv[i]  +  dytv[i]  +  2.0  *  dymv[i]); 

} 

ytemp[3]  =  ysav[3]  +  h61  *  (dydx[3]  +  dyt[3]  +  20*  dym[3]), 

//error  determination 
errmax  =  0.0; 
for(i  =  0;  i  <  3;  i++) 

{ 

ytemp[i]  =  ytemp2[i]  -  ytemp[i]; 

if(errmax  <  fabs(ytemp[i])) 

errmax  =  fabs(ytemp[i]); 
ytempa[i]  =  ytemp2a[i]  -  ytempa[i]; 

if(errmax  <  fabs(ytempa[i])) 

errmax  =  fabs^empa[i]); 
ytempl[i]  =  ytemp21[i]  -  ytempl[i]; 
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if(errmax  <  fabs(ytempl[i])) 

errmax  =  fabs(ytempl[i]); 
ytempv[i]  =  ytemp2v[i]  -  ytempv[i]; 

if(errmax  <  fabs(ytempv[i])) 

errmax  =  fabs(ytempv[i]); 

} 

ytemp[3]  =  ytemp2[3]  -  ytemp[3]; 

if(errmax  <  fabs(ytemp[3])) 

errmax  =  fabs(ytemp[3]); 
errmax  /=  eps; 

if(errmax  <  1.0) 
break; 

h  =  SAFETY  *  h  *  exp(PSHRNK*log(errmax)); 

} 

for(i  =  0;  i  <  3;  i-H-) 

{ 

orientation[i]  =  ytemp2[i]  +  ytemp[i]  *  FCOR; 
ang_velocity[i]  =  ytemp2a[i]  +  ytempa[i]  *  FCOR; 
(*location)[i]  =  ytemp21[i]  +  ytemplfi]  *  FCOR; 
velocity[i]  =  ytemp2v[i]  +  ytempv[i]  *  FCOR; 

} 

orientation[3]  =  ytemp2[3]  +  ytemp[3]  *  FCOR; 

orientation.normalizeO; 

force[0]  =  0.0; 
force[l]  =  0.0; 
force[2]  =  0.0; 
moment[0]  =  0.0; 
moment[l]  =  0.0; 
moment[2]  =  0.0; 
return  h; 


void  rigid_body;  .display0 

{ 

Matrix  rt  =  {  1.0,  0.0,  0.0,  0.0, 
0.0,  1.0,  0.0,  0.0, 
0.0,  0.0,  1.0,  0.0, 
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0  0,  0.0,  0  0,  1,0}; 


Matrix  scale  -  {  1.0,  0,0,  0,0,  0,0, 
0.0,  1.0,  0  0,  0.0, 
0.0,0  0,  1.0,  0.0, 
0.0,  0.0,  0.0,  1.0}, 


pushmatrixQ; 

rt[0][0]  =  ((orientation[0]  *  orientation[0])  +  (orientation}  1]  *  orientation}  1  ])  - 

(orientation}2]  *  orientation}2])  -  (orientation}3]  *  orientation}3])); 
ri}l]}0]  -  2.0  *  ((orientation}!]  *  orientation}2])  -  (orientation}0]  * 
orientation}3]));  rt}2]}0]  =  2  0  *  ((orientation}0]  *  orientation}2])  +  (orientation}!] 

*  orientation}3])); 

rt}0]}l]  =  2.0  *  ((orientation}!]  *  orientation}2])  +  (orientation}0]  * 

orientation}3])); 

rt}l]}l]  =  ((orientation}0]  *  orientation}©])  -  (orientation}!]  *  orientation}!])  + 

(orientation}2]  *  orientation}2])  -  (orientation}3]  *  orientation}3])); 
rt}2]}l]  =  2.0  *  ((orientation}2]  *  orientation}3])  -  (orientation}©]  * 
orientation}  1  ])); 

ri}0]}2]  =  2.0  *  ((orientation}!]  *  orientation}3])  -  (orientation}©]  * 
orientation}2])); 

rt}l]}2]  =  2.0  *  ((orientation}2]  *  orientation}3])  +  (orientation}©]  * 

orientation}!])); 

rt}2]}2]  =  ((orientation}©]  *  orientation}©])  -  (orientation}!]  *  orientation}!])  - 

(orientation}2]  •  orientation}2])  +  (orientation}3]  *  orientation}3])); 
rt}3]}0]  =  (^location)}©]; 
rt}3]}!]  =  (*location)}!]; 
rt[3]}2]  =  (*location)}2]; 
multmatrix(rt); 
scale}©]  }0]  =  size}©]; 
scale}!]}!]  =  size}!]; 
sca!e}2]}2]  =  size}2]; 
multniatrix(sca!e); 

if(display_axis  &&  *disp!ay_shape) 

{ 

viewaxisO; 

} 

if(*display_shape) 

{ 

display_this_object(shape); 

} 
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popmatrix(), 

} 


void  rigid_body::add_axis() 

{ 

display  axis  =  1 ; 


void  rigid_body::remove_axis() 

{ 

displayaxis  =  0; 

} 


void  rigid_body:  :attach_eye() 

{ 

attach_eye_to(locatioii,  display  shape); 


void  rigid_body:  .attach_targetO 

{ 

attach_target_to(location); 

} 


void  set_eye(double  x,  double  y,  double  z) 

{ 

set_eye_to(x,  y,  z); 

} 


void  set_target(double  x,  double  y,  double  z) 

{ 

set_target_to(x,  y,  z); 

} 


void  rigid_body::zeroO 

{ 

vectorSD  zerovector; 
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size[0]  =1.0; 
size[l]  =  10; 
size[2]  =  1 .0; 

*  location  =  zero  vector; 
velocity  =  zerovector; 
acceleration  =  zerovector; 
orientation[0]  =1.0; 
orientation[l]  =  0.0; 
orientation[2]  =  0.0, 
orientation[3]  =  0.0; 
angvelocity  =  zerovector; 
angacceleration  =  zerovector; 


void  rigid_body.  :attached_body_update(rigid_body  r) 

{ 

vectors  D  av; 
matrixSxS  rotation; 

*  location  =  holder  1; 
update_state(); 
holderl  =  *location; 

rotation.DCM_body_to_world(r.orientation); 
*location  =  (rotation  *  (*location))  +  ♦r.location; 
holder2  =  (rotation  *  r.ang  velocity)  +  r.holder2; 
av  =  holder2; 

rotation.DCM_world_to_body(orientation); 
av  =  rotation  *  av; 
orientation.update(av,read_deltaO); 

} 

#endif 
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APPENDIX  D.  VECTOR3DCODE 


A.  HEADER  FILE 

#ifhdefVECT0R3D_H 
#define  VECT0R3D_H 
#include  <iostream.h> 

#include  <math.h> 

class  vectorSD 

I 

I 

double  x; 
double  y; 
double  z;  public; 
vector3D(); 

vectors D(double,  double,  double); 
vectors  D(const  vectorSD&); 
void  zeroQ; 

vectors  D&  operator=(const  vectorSD&); 
vectors  D  operator+(const  vectoiSD&); 
vectorSD  operator-(const  vectorSD&); 

double  operator*(const  vectorSD&);  //dot  product 

vectorSD  operator*(double);  //scalar  multiplication 

vectorSD  operator/(double);  //scalar  division 

vectorSD  operator'Xconst  vectorSD&);  //cross  product 

double  magnitudeO; 

void  normalizeO; 

void  normalize(double); 

fiiend  ostream&  operator«(ostream&,  vectorSD&); 
double&  operator[](int), 

-vectorSDQ  {  } 

}; 

#endif 

B.  SOURCE  FILE 

#ifhdefVECTOR3D_C 
#define  VECTORSD_C 
#include  "vectorSD.H" 

//default  constructor  vectorSD:  ivectorSDQ 

{ 
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x  =  00, 
y  =  00. 
z  =  00. 


//constructor  using  three  doubles 

vectorSD  vector3D(doubIe  a,  double  b,  double  c) 

{ 

X  =  a, 
y  =  b, 
z  =  c, 

} 


//constructor  using  another  vectorSD 
vector3D;;vector3D(const  vector3D&  v) 
{ 

X  =  v.x; 
y  =  v.y; 
z  =  v.z; 

} 


//zero  out  the  vector 
void  vector3D;:zeroO 
{ 

X  =  0.0; 
y  =  0.0; 
z  =  0.0; 

} 


//Assignment  operator  -  the  function  must  return  a  reference  to  a  vector 
//instead  of  a  vector  for  assignment  to  work  properly 
vector3D&  vector3D:;operator=(const  vector3D&  v) 

{ 

X  =  v.x; 

y  =  v.y; 
z  =  v.z; 
return  *this; 

} 
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vector  addition  operator 

vector3D  vectorlD  operator*! const  vector3D&  \ ) 

! 

vector3D  sum. 
sum  X  =  V  X  *  X. 
sum  y  =  V  y  *  y. 
sum  z  =  V  z  *  z, 
return  sum, 

} 


//vector  substraction 

vectorSD  vector3D::operator-(const  vector3D&  v) 

{ 

vector3D  difF; 
difFx  =  X  -  v.x; 
difF.y  =  y  -  v.y; 
difF.z  =  z  -  v.z; 
return  diff; 

} 


//vector  dot  product 

double  vector3D::operator*(const  vector3D&  v) 

{ 

double  dot; 

dot  =  (v.x  *  x)  +  (v.y  *  y)  +  (v.z  *  z); 
return  dot; 

} 


//scalar  multiplication  vector3D  vector3D::operator*(double  n) 

{ 

vector3D  mult; 
mult.x  =  x  *  n; 
mult.y  =  y  *  n; 
mult.z  =  z  *  n; 
return  mult; 

} 
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scalar  division  -  it  is  the  user  responsibiiitv  to  make  sure  that  r.  is  not  zero  vector3D 
vectoiJD  operator/! double  n) 

I 

vcctor3D  result, 
result  X  =  X  /  n, 
result  y  =  y  /  n, 
result  z  =  z  /  n, 
return  result. 


//vector  cross  product 

vector3D  vector3D::operator^const  vector3D&  v) 

{ 

vector3D  cross; 
cross.x  =  (y  *  v.z)  -  (v.y  *  z); 
cross.y  =  -((x  *  v.z)  -  (v.x  *  z)); 
cross.z  =  (x  *  v.y)  -  (v.x  *  y); 
return  cross; 

} 


//the  «  operator  is  to  be  used  with  cout 
ostream&  operator«(ostream&  os,  vector3D&  v) 

{ 

os  «  (double)  v.x  «  ", "  « (double)  v.y  «  ", "  «  (double)  v.z  «  "\n"; 
return  os; 

} 


//allows  access  to  the  components  of  the  vector3D.  it  must  return  a  reference 
//in  order  for  assignment  to  work 
double&  vector3D:.operator[](int  n) 

{ 

if  (n  =  0) 

{ 

return  x; 

) 

if(n=  1) 

{ 

return  y; 

} 

if  (n  =  2) 


104 


t 

return  z. 

I 


//returns  the  magnitude  of  the  vector 
double  vector3D  magnitude() 

{ 

return  sqrt((x  *  x)  +  (y  *  y)  +  (z  *  z)). 


//normalizes  the  vector  to  one 
void  vector3D::normali2e() 

{ 

double  m  =  sqrt((x  *  x)  +  (y  *  y)  (z  *  z)), 
if(m) 

{ 

X  =  X  /  m; 


y  =  y  /  m, 
z  =  z  /  m, 


} 


} 


//normalize  the  vector  to  d 
void  vector3D:  :normalize(double  d) 

{ 

double  m  =  sqrt((x  *  x)  +  (y  •  y)  +  (z  *  z)), 
if(m) 

{ 

X  =  d  •  X  /  m; 
y  =  d  *  y  /  m; 
z  =  d  *  z  /  m, 

} 

} 


#endif 
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APPENDIX  E.  MATRIX3X3  CODE 


A.  HEADER  FILE 

#ifhdefMATRIX3X3_H 
#define  MATRIX3X3_H 
#include  "vector3D.H'' 

^include  "quatemic  n.H" 

#include  <iostream.h> 

class  matrix3x3 

{ 

double  m[9]; 

public 

matrix3x3(); 

matrix3x3(double,  double,  double,  double,  double,  double,  double,  double, 
double); 

matrix3x3(const  matrix3x3&), 
void  resetQ; 

matrix3x3&  operator=(const  matrix3x3&); 
matrix3x3  operator+(const  matrix3x3&); 
matrix3x3  operator-(const  matrix3x3&); 
matrix3x3  operator*(const  matrix3x3&); 
void  DCM_x_rotation(double); 
void  DCM_y_rotation(double), 
void  DCM_z_rotation(double); 
void  DCM_body_to_world(quatemion); 
void  DCM_worId_to_body(quatemion); 
void  transposeO; 

quaternion  generateorientationQ; 
vector3D  operator*(vector3D&); 
matrix3x3  operator*(double), 
double&  operator[](int); 

friend  ostreani&  operator«(ostream&,  matrix3x3&); 

~niatrix3x30  {  } 

}; 


#endif 


B.  SOURCE  FILE 

#ifhdefMATRIX3X3_C 
#define  MATRIX3X3  C 
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^include  "matrix3x3  H 


mat  rix3  x3 : :  matrix3  x3  0 

{ 

m[0]-  1, 
m[l]  =  0; 
m[2]  =  0; 
m[3]  =  0; 
m[4]  =  1, 
m[5]  =  0; 
m[6]  =  0; 
m[7]  =  0, 
m[8]=  1; 

} 


matrix3x3 :  ;matrix3x3(double  a,  double  b,  double  c,  double  d,  double  e,  double  f,  double  g, 

double  h,  double  i) 


{ 

m[0]  =  a; 
m[l]  =  b; 
m[2]  =  c; 
m[3j  =  d; 
m[4]  =  e; 
m[5]  =  f; 
m[6]  =  g; 
in[7]  =  h; 
m[8]  =  i; 

} 


matrix3x3:;matrix3x3(const  matrix3x3&  v) 

{ 

m[0]  =  v.m[0]; 
m[l]  =  v.m[l]; 
m[2]  =  v.in[2j; 
m[3]  =  v.m[3]; 
m[4]  =  v.m[4]; 
m[5]  =  v.m[5]; 
m[6]  =  v.m[6]; 
m[7]  =  v.m[7]; 
m[8]  =  v.m[8]; 

} 
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void  matrix3x3::reset() 

{ 

m[0]  =  1; 
m[l]  =  0; 
m[2]  =  0, 
m[3]  =  0; 
m[4]  =  1, 
m[5]  =  0; 
m[6]  =  0; 
m[7]  =  0; 
m[8]=l; 

} 


matrix3x3&  matrix3x3:  :operator=(const  matrix3x3&  v) 

{ 

m[0]  =  V  m[0], 
m[l]  =  v.m[l], 
m[2]  =  v.mt2]; 
m[3]  =  v.m[3]; 
m[4]  =  vjmt4]; 
in[5]  =  v.m[5]; 
m[6]  =  v.in[6]; 
ni[7]  =  V  m[7]; 
ni[8]  =  v.m[8]; 
return  *this; 


matrix3x3  matrix3x3:;operatorKconst  matrix3x3&  v) 


matrix3x3 

sum. 

sum.m[0] 

=  m[0] 

+  v.m[0]; 

suin.m[l] 

=  m[l] 

+  v.m[l]; 

suin.m[2] 

=  m[2] 

+  v.m[2]; 

suni.m[3] 

=  m[3] 

+  v.m[3]; 

sum.m[4] 

=  m[4] 

+  v.m[4]; 

suin.ni[5] 

=  m[5] 

+  v.misj; 

sum.ni[6] 

=  m[6] 

+  v.m[6]; 

sum.m[7] 

=  m[7] 

+  v.m[7]; 

sum.m[8] 

=  m[8] 

+  v.m[8]; 
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return  sum; 


} 


matrix3x3  matrix3x3 :  operator-( const  matrix3x3&  v) 

{ 

matrix3x3  difference, 
difference. m[0]  =  m[0]  -  v.m[0], 
difference.m[l]  =  ni[l]  -  v  m[l], 
difference. m[2]  =  ni[2]  -  v.m[2], 
difference. m[3]  =  m[3]  -  v.m[3], 
difference.  m[4]  =  m[4]  -  v  m[4]; 
difference.m[5]  =  m[5]  -  v.in[5], 
diflference.m[6]  =  m[6]  -  v.m[6], 
difference. m[7]  =  m[7]  -  v.m[7]; 
difference.m[8]  =  m[8]  -  v.m[8], 
return  difference; 

} 


matrix3x3  matrix3x3;;operator*(const  matrix3x3&  v) 

{ 

matrix3x3  temp; 

temp.m[0]  =  (m[0]  *  v.m[0])  +  (m[l]  *  v.m[3])  +  (m[2]  *  v.m[6]); 
temp.m[l]  =  (mtO]  ♦  v.m[l])  +  (m[l]  *  v.m[4])  +  (m[2]  •  v.m[7]); 
temp.m[2]  =  (m[0]  ♦  v.m[2))  +  (m[l]  *  v.m[5])  +  (m[2]  *  v.m[8]); 
temp.m[3]  =  (m[3]  *  v.m[0])  +  (m[4]  *  v.m[3])  +  (m[5]  *  v.m[6]); 
temp.m[4]  =  (m[3]  •  v.m[l])  +  (m[4]  •  v.m[4])  +  (mI5]  *  v.m[7]); 
temp.m[5]  =  (m[3]  *  v.m[2])  +  (m(4]  *  v.m[5])  +  (m[5]  *  v.m[8]); 
temp.m[6]  =  (m[6]  •  v.m[0])  +  (m[7]  *  v.m[3])  +  (m[8]  *  v.m[6]); 
temp.m[7]  =  (mt6]  *  v.m[l])  +  (m[7]  •  v.m[4])  +  (m[8]  •  v.m[7]); 
temp.m[8]  =  (m[6]  *  v.m[2])  +  (m[7]  *  v.m[5])  +  (m[8]  *  v.m[8]); 
return  temp; 

} 


vector3D  matrix3x3;:operator*(vector3D&  v) 

{ 

vector3D  temp  =  v; 

temp[0]  =  (m[0]  *  v[0])  +  (m[l]  *  v[l])  +  (m[2]  ♦  v[2]); 
temp[l]  =  (m[3]  *  v[0])  +  (m[4]  •  v[l])  +  (m[5]  •  v[2]); 
temp[2]  =  (m[6]  •  v[01)  +  (m[7]  *  v[l])  +  (m[8]  •  v[2]); 
return  temp; 
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) 


{ 


} 


matrix3x3  matrix3x3::operator*(double  n) 

niatrix3x3  product; 
product.  m[0]  =  m[0]  *  n, 
product.m[l j  =  m[l]  *  n; 
product.m[2]  =  m[2]  *  n; 
product. m[3]  =  m[3]  *  n; 
product.  m[4]  =  m[4]  *  n; 
product.m[5]  =  m[5]  *  n; 
product. m[6]  =  m[6]  *  n; 
product.  m[7]  =  m[7]  *  n, 
product.m[8]  =  m[8]  *  n; 
return  product; 


ostream&  operator«(ostream&  os,  matrix3x3&  v) 

{ 

os  « (double)  v.m[0]  «  ", "  «  (double)  v.in[l]  «  ", "  « (double)v.m[2] 
«"\n" 

«  (double)  v.m[3]  «  ", "  « (double)  v.m[4]  «  ", "  «  (double)  v.m[5] 
« "\n" 

« (double)  v.m[6]  «  ", "  « (double)  v.m[7]  «  ", "  « (double)  v.in[8] 
« "\n" « "\n"; 
return  os; 

} 


double&  niatrix3x3::operator[](int  y) 

{ 

return  m[y]; 

} 


void  matrix3x3;:DCM_x_rotation(double  angle) 

{ 

in(0]  =  1.0; 
m[l]  =  0.0; 
m[2]  =  0.0; 
m[3]  =  0.0; 
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m[4]  =  cos(angle); 
m[5]  =  sin(  angle); 
m[6]  =  0.0; 
m[7]  =  -sin(angle); 
ni[8]  -  cos(angle), 

} 


void  matrix3x3;;DCM_y_rotation(double  angle) 

{ 

m[0]  =  cos(angle); 
m[l]  =  0.0; 
m[2]  =  -sin(angle); 
m[3]  =  0.0; 
m[4]=  1.0; 
m[5]  =  0.0; 
m[6]  =  sin(angle); 
m[7]  =  0.0; 
m[8]  =  cos(angle); 

} 


void  niatrix3x3::DCM_z_rotation(doubIe  angle) 

{ 

m[0]  =  cos(angle); 
ni[l]  =  sin(an^e); 
m[2]  =  0.0; 
ni[3]  =  -sin(angle); 
m[4]  =  cos(angle); 
m[5]  =  0.0; 
m[6]  =  0.0; 
in[7]  =  0.0; 
m[8]=  1.0; 

} 


void  matrix3x3;;DCM_body_to_world(qiiatemion  orientation) 

ni[0]  =  ((orientation[0]  *  orientation[0])  +  (orientation[l]  *  orientation[l])  - 
(orientation[2]  *  orientation[2])  -  (orientation[3]  *  orientation[3])); 
m[l]  =  2.0  •  ((orientation[l]  *  orientation[2])  -  (orientation[0]  •  orientation[3])); 
m[2]  =  2.0  *  ((orientation[0]  *  orientation[2])  +  (orientation[l]  *  orientation[3])); 
m[3]  =  2.0  *  ((orientation[l]  *  orientation[2])  +  (orientation[0]  *  orientation[3])); 
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m[4]  =  ({onentation[0]  *  orientation[0])  -  (orientation!  1]  *  orientation!  1])  + 
(orientation!2]  *  orientation!2])  -  (orientation!3]  *  orientation!3])), 
m!5]  =  20*  ((orientation!2]  *  orientation!3])  -  (orientation!0]  *  orientation!!])). 
m!6]  =  2  0  *  ((orientation!!]  *  orientation!3])  -  (orientation!0]  *  orientation!2])), 
m!?]  =  2  0  *  ((orientation!2]  *  orientation[3])  (orientation!0]  *  orientation!!])); 
m!8]  =  ((orientation!0]  *  orientation!0])  -  (orientation!!]  *  orientation!!])  - 
(orientation!2]  *  orientation!2])  +  (orientation!3]  *  orientation!31)). 


void  matrix3x3  :DCM_world_to_body(quatemion  orientation) 

i 

\ 

m[0]  =  ((orientation!0]  *  orientation[0])  +  (orientation!!]  *  orientation!!])  - 
(orientation(2]  *  orientationI2])  -  (orientation!3]  *  orientation!3])); 
m!3]  =  2.0  *  ((orientation!  1  ]  *  orientation[2])  -  (orientation!©]  *  orientation!3])); 
m!6]  =  2.0  *  ((orientation[0]  *  orientation[2])  +  (orientation!!]  *  orientation!3])); 
m!!]  =  2.0  *  ((orientation!!]  *  orientation[2])  +  (orientation!0]  *  orientation[3])), 
m[4]  =  ((orientation!0]  *  orientation[0])  -  (orientation!!]  *  orientation!!])  + 
(orientation!2]  *  orientation!2])  -  (orientation!3]  *  orientation!3])), 
m!7]  =  2.0  *  ((orientation!2]  *  orientation!3j)  -  (orientation!0]  *orientation!!])); 
tn!2]  =  2.0  *  ((orientation!!]  *  orientation!3])  -  (orientationlO]  *  orientation!2])); 
ni!5]  =  2.0  *  ((orientation!2]  *  orientation!3])  +  (orientation!©]  *  orientation!!])); 
m!8]  =  ((orientation!©]  •  orientation!©])  -  (orientation! ! ]  *  orientation!!])  - 
(orientation!2]  *  orientation!2])  -r  (orientation!3]  * 

orientation!3])); 

} 


void  matrix3 a3  :  :transpose() 

{ 

matrix3x3  v  =  *this; 
m[0]  -  v.m!0]; 
m!!]  =  v.m!3]; 
m!2]  =  v.m!6]; 
m!3]  =  v.m!!], 
m!4]  =  v.m!4]; 
mI5]  =  v.m!?]; 
m[6]  =  v.m!2]; 
m!?]  =  v.mI5]; 
m!8]  =  v.m!8]; 

} 
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quaternion  matrix3x3 :  :generate_orientation() 

{ 

quaternion  q;. 

q[0]  =  0.5  *  sqrt(l  +  m[0]  +  m[4]  +  m[8]); 
q[l]  =  0.5  *  sqrt(I  +  m[0]  -  in[4]  -  in(8]); 
q[2]  =  0.5  *  sqrt(l  -  m[0]  +  ni[4]  -  m[8]); 
q[3]  =  0.5  *  sqrt(l  -  m[0]  -  m[4]  +  m[8]); 
q.normalizeO; 
return  q; 

} 


#endif 
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APPENDIX  F.  QUATERNION  CODE 


A.  HEADER  FILE 

#ifhdef  QU  ATERN10N_H 
#define  QUATERNION  H 
#include  <iostream.h> 
#include  <math.h> 

^include  "vectorSD.H" 

class  quaternion 

{ 

double  qO; 
double  ql; 
double  q2; 
double  q3; 


public; 

quatemionO; 

quatemion(double,  double,  double,  double); 
quatem  ion(const  quatemion&); 
void  set(double,  double,  double,  double); 
quatemion&  operator=(const  quatemion&); 
quaternion  operator+(const  quatemion&); 
quaternion  operator-(const  quatemion&); 
quaternion  operator*(const  quatemion&); 
quaternion  operator*(double); 
double&  operator[](int); 
double  magnitudeQ; 
void  normalizeO; 

quaternion  rate_of_change(double,  double,  double); 

void  update(double,  double,  double,  double); 

quaternion  rate_of_change(vector3D); 

void  update(vector3D,  double); 

friend  ostreain&  operator«(ostreani&,  quatemion&); 

-quatemionO  {  } 


#endif 

B.  SOURCE  FILE 
#ifiidef  QU  ATERNIONC 
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#define  QUATERNION_C 
#include  "quaternion. H" 

quaternion:  :quatemion() 

{ 

q0=1.0; 
ql=0.0; 
q2  =  0.0; 
q3  =  0.0; 

} 


quaternion;  ;quatemion(double  angle  x,  double  angle_y,  double  angle  z,  double  rotation) 

{ 

qO  =  cosf(0.5*rotation); 
ql  =  cosf(angle_x)*sinf(0.5*rotation); 
q2  =  cosf(angle_y)*sin£(0,5*rotation); 
q3  =  cosf(angle_z)*sinf(0.5*rotation); 

} 


quaternion;  ;quatemion(const  quatemion&  q) 

{ 

qO  =  q.qO; 
ql  =q.ql; 
q2  =  q.q2; 
q3  =  q.q3; 

} 


void  quaternion:  ;set(double  angle_x,  double  8ngle_y,  double  angle_z,  double  rotation) 

{ 

qO  =  cosf(0.5*rotation); 
ql  =  cosf(angle_x)*sinf(0.5*rotation); 
q2  =  cosf(angle_y)*sinf(0.5*rotation); 
q3  =  cosf(angle_z)*sinf(0. 5  •rotation); 

} 


quatemion&  quaternion;  ;operator=(const  quatemion&  q) 

{ 

qO  =  q.qO; 
ql  =q.ql; 
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q2  =  q.q2; 
q3  =  q.q3; 
return  *this, 

} 


quaternion  quaternion:  :operator-t-(const  quatemion&  q) 

{ 

quaternion  sum; 
sum.qO  =  qO  +  q.qO; 
sum.ql  =  ql  +  q.ql; 
sum.q2  =  q2  +  q.q2; 
sum.q3  =  q3  +  q.q3; 
return  sum; 

} 


quaternion  quaternion;  ;operator-(const  quatemion&  q) 

{ 

quaternion  diff; 
diflf.qO  =  qO  -  q.qO; 
difF.ql  =  ql  -  q.ql; 
diflf.q2  =  q2  -  q.q2; 
difF.qS  =  q3  -  q.q3; 
return  difF; 

} 


quaternion  quaternion;  ;operator*(const  quatemion&  q) 

{ 

quaternion  prod; 

prod.qO  =  (qO  *  q.qO)  -  (ql  •  q.ql)  -  (q2  ♦  q.q2)  -  (q3  *  q.q3); 
prod.ql  =  (ql  *  q.qO)  +  (qO  *  q.ql)  -  (q3  *  q.q2)  +  (q2  *  q.q3); 
prod.q2  =  (q2  *  q.qO)  +  (q3  *  q.ql)  -  (qO  *  q.q2)  +  (ql  *  q.q3); 
prod.q3  =  (q3  *  q.qO)  +  (q2  *  q.ql)  -  (ql  ♦  q.q2)  +  (qO  *  q.q3); 
return  prod; 

} 


quaternion  quaternion;; operator *(double  n) 

{ 

quaternion  prod; 
prod.qO  =  qO  *  n; 


117 


} 


prod.ql  =  ql  *  n; 
prod.q2  =  q2  *  n, 
prod.q3  =  q3  *  n; 
return  prod; 


double  quaternion;  ;magnitudeO 

{ 

return  sqrt((qO  *  qO)  +  (ql  *  ql)  +  (q2  *  q2)  +  (q3  *  q3)); 

} 


void  quaternion  :  .normalize() 

{ 

double  m  =  sqrt((qO  *  qO)  +  (ql  *  ql)  +  (q2  *  q2)  +  (q3  *  q3)); 
if(m) 

{ 

qO  =  qO  /  m; 
ql  =  ql  /m; 
q2  =  q2  /  m; 
q3  =  q3  /  m; 

} 

} 


quaternion  quaternion:  ;rate_o^change(double  P,  double  Q,  double  R) 

{ 

quaternion  rate; 

rate.qO  =  -0.5*((ql  *  P)  +  (q2  *  Q)  +  (q3  *  R)); 
rate.ql  =  0.5*((q0  *  P)  +  (q2  *  R)  -  (q3  ♦  Q)); 
rate.q2  =  0.5*((q0  •  Q)  +  (q3  •  P)  -  (ql  *  R)); 
rate.q3  =  0.5*((q0  *  R)  +  (ql  •  Q)  -  (q2  •  P)); 
return  rate; 

} 


void  quaternion;  ;update(double  P,  double  Q,  double  R,  double  sec) 

( 

double  hh  =  sec  *  .5,  h6  =  sec  /  6; 
quaternion  y  =  *this,  dym,  dyt,  yt,  dydx; 
dydx  qO  =  -0.5*((ql  *  P)  +  (q2  *  Q)  +  (q3  *  R)); 
dydx.ql  =  0.5*((q0  *  P)  +  (q2  *  R)  -  (q3  •  Q)); 
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dydx.qa  =  0.5*((q0  *  Q)  +  (q3  *  P)  -  (ql  *  R)), 

dydx.q3  =  0.5*((q0  *  R)  +  (ql  *  Q)  -  (q2  •  P)), 

yt.qO  =  y.qO  +  hh  *  dydx.qO; 

yt.ql  =  y.ql  +  hh  *  dydx.ql; 

yt.q2  =  y.q2  +  hh  *  dydx.q2; 

yt.q3  =  y.q3  +  hh  *  dydx.q3; 

dyt.qO  =  -0.5*((yt.ql  *  P)  +  (yt.q2  *  Q)  +  (yt.q3  *  R)); 
dyt.ql  =  0.5*((yt.q0  *  P)  +  (yt.q2  *  R)  -  (yt.q3  •  Q)); 
dyt.q2  =  0.5*((yt.q0  *  Q)  +  (yt.q3  *  P)  -  (yt.ql  *  R)); 
dyt.q3  =  0.5*((yt.q0  *  R)  +  ^.ql  *  Q)  -  (yt.q2  *  P)); 
yt.qO  =  y.qO  +  hh  *  dyt.qO; 
yt.ql  =  y  ql  +  hh  *  dyt.ql; 
yt.q2  =  V  q2  +  hh  *  dyt.q2; 
yt.qS  =  y.q3  +  hh  *  dyt.q3; 

dym.qO  =  -0.5*((yt.ql  *  P)  +  (yt.q2  *  Q)  +  (yt.q3  *  R)); 

dym.qi  =  0.5*((yt.q0  *  P)  +  (yt.q2  *  R)  -  (yt.q3  *  Q)); 

dym.q2  =  0.5*((yt.q0  *  Q)  +  (yt.q3  ♦  P)  -  (yt.ql  *  R)); 

dym.q3  =  0.5*((yt.q0  *  R)  +  (yt.ql  *  Q)  -  (yt.q2  *  P)); 

yt.qO  =  y.qO  +  sec  *  dym.qO; 

yt.ql  =  y.ql  +  sec  *  dym.qi; 

yt.q2  =  y.q2  +  sec  *  dym.q2; 

yt.q3  =  y.q3  +  sec  *  dym.q3; 

dym.qO  =  dym.qO  +  dyt.qO; 

dym.qi  =  dym.qi  +  dyt.ql; 

dym.q2  =  dym.q2  +  dyt.q2; 

dym.q3  =  dym.q3  +  dyt.qS; 

dyt.qO  =  -0.5*(^.ql  *  P)  +  (yt.q2  *  Q)  (yt.q3  *  R)); 
dyt.ql  =  0.5*((yt.q0  *  P)  +  (yt.q2  *  R)  -  (yt.q3  *  Q)); 
dyt.q2  =  0.5*((yt.q0  *  Q)  +  (yt.q3  *  P)  -  (yt.ql  *  R)); 
dyt.q3  =  0.5*((yt.q0  *  R)  +  (yt.ql  *  Q)  -  (yt.q2  *  P)); 
qO  =  y.qO  +  h6  *  (dydx.qO  +  dyt.qO  +  2.0  *  dym.qO); 
ql  =  y.ql  +  h6  *  (dydx.ql  +  dyt.ql  +  2.0  *  dym.qi); 
q2  =  y.q2  +  h6  *  (dydx.q2  +  dyt.q2  +  2.0  *  dym.q2); 
q3  =  y.q3  +  h6  *  (dydx.qS  +  dyt.qS  +  2.0  *  dym.q3); 


quaternion  quaternion:  :rate_of_change(vector3D  ang  velocity) 

{ 


quaternion  rate; 

rate.qO  =  -0.5*((ql  *  ang_velocity[0])  +  (q2  *  ang_velocity[l])  +  (q3  * 
ang_velocity[2])); 
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rate  ql  -  0,5*((q0  *  ang_velocity[0])  +  (q2  *  ang_velocity[2])  -  (q3  * 
ang_velocity[l])), 

rate  q2  =  0.5*((q0  *  arg  velocity[l])  +  (q3  *  ang_velocity[0])  -  (ql  * 
ang_velocity[2]));  rate.q3  =  0.5*((q0  *  ang_veIocity[2])  +  (ql  * 

ang_velocity[l])  -  (q2  *  ang_velocity[0])); 
return  rate; 


void  quaternion:  ;update(vector3D  ang_velocity,  double  sec) 

double  P  =  ang_velocity[0],  Q  =  ang_velocity[l],  R  =  ang_velocity[2]; 

double  hh  =  sec  *  .5,  h6  =  sec  /  6, 

quaternion  y  =  *this,  dynt,  dyt,  yt,  dydx; 

dydx.qO  =  -0.5*((ql  *  P)  +  (q2  *  Q)  +  (q3  *  R)); 

dydx.ql  =  0.5*((q0  *  P)  +  (q2  *  R)  -  (q3  *  Q)); 

dydx.q2  =  0.5*((q0  *  Q)  +  (q3  *  P)  -  (ql  •  R)); 

dydx.q3  =  0.5*((q0  *  R)  +  (ql  *  Q)  -  (q2  *  P)); 

yt.qO  =  y.qO  +  hh  *  dydx.qO; 

yt.ql  =  y.ql  +  hh  *  dydx.ql; 

yt.q2  =  y.q2  +  hh  *  dydx.q2; 

yt  <13  =  y.q3  +  hh  *  dydx.q3; 

dyt.qO  =  -0.5*((yt.ql  *  P)  +  (yt.q2  *  Q)  +  (yt.q3  *  R)); 
dyt.ql  =  0.5*((yt.q0  *  P)  +  (yt.q2  *  R)  -  (yt.q3  *  Q)); 
dyt  q2  =  0.5*((yt.q0  *  Q)  +  (yt.qS  *  P)  -  (yt.ql  •  R)); 
dyt.q3  =  0.5*((yt.q0  *  R)  +  (yt.ql  *  Q)  -  (yt.q2  *  P)); 
yt.qO  =  y.qO  +  hh  *  dyt.qO; 
yt.ql  =y.ql  +hh  *  dyt.ql; 
yt.q2  =  y.q2  +  hh  *  dyt.q2; 
yt.q3  =  y.q3  +  hh  *  dyt.q3; 

dym.qO  =  -0.5*((yt.ql  *  P)  +  (yt.q2  *  Q)  +  (yt.q3  *  R)); 

dym.ql  =  0.5*((yt.q0  *  P)  +  (yt.q2  ♦  R)  -  (yt.q3  *  Q)); 

dym.q2  =  0.5*((yt.q0  •  Q)  +  (yt.qS  *  P)  -  (yt.ql  •  R)); 

dym.q3  =  0.5*((yt.q0  *  R)  +(yt.ql  ♦  Q)  -  (yt.q2  •  P)); 

yt.qO  =  y.qO  +  sec  *  dym.qO; 

yt.ql  =  y.ql  +  sec  *  dym.ql; 

yt.q2  =  y.q2  +  sec  *  dym.q2; 

yt.q3  =  y.q3  +  sec  *  dym.q3; 

dym.qO  =  dym.qO  +  dyt.qO; 

dym.ql  =  dym.ql  +  dyt.ql; 

dym.q2  =  dym.q2  +  dyt.q2; 

dym.q3  =  dym.q3  +  dyt.q3; 

dyt.qO  =  .0.5*((yt.ql  *  P)  +  (yt.ql  *  Q)  +  (yt.q3  *  R)); 
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dyt  ql  =  0  5*((yt  qO  *  P)  +  (yt  q2  *  R)  -  (yt  q3  *  Q)); 
dyt.q2  =  0.5*((yt.q0  *  Q)  +  (yt  q3  *  P)  -  (yt.ql  *  R)); 
dyt.q3  =  0.5*((yt.q0  *  R)  +  (yt.ql  *  Q)  -  (yt.q2  *  P)), 
qO  =  y.qO  +  h6  *  (dydx.qO  +  dyt.qO  +  2.0  *  dym.qO), 

ql  =  y.ql  +  h6  *  (dydx.ql  +  dyt.ql  +  2.0  *  dym.ql); 

q2  =  y.q2  +  h6  *  (dydx.q2  +  dyt.q2  +  2.0  *  dym,q2), 

q3  =  y.q3  +  h6  *  (dydx.q3  +  dyt.q3  +  2.0  *  dym.q3), 

} 


ostream&  operator«(ostream&  os,  quatemion&  q) 

{ 

os  «  (double)  q.qO  «  ", "  «  (double)  q.ql  «  ", "  «  (double)  q.q2  «  ",  " 
«  (double)  q.q3  «  "\n"; 
return  os; 

} 


double&  quaternion:  :operator[](int  n) 

{ 

if  (n  =  0) 

{ 

return  qO; 

} 

if(n==l) 

{ 

return  ql; 

} 

if(n  =  2) 

{ 

return  q2; 

} 

if(n  =  3) 

{ 

return  q3; 

} 

} 

#endif 
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APPENDIX  G.  MENU  CODE 


A.  HEADER  FILE 

#ifTKiefMENU_H 
#define  MENUH 
#include  "menu.H" 
#include  <gl.h> 

#include  <device.h> 
^include  <stdio.h> 
^include  <stdiib.h> 

void  initialize_menuO; 
int  queue  testQ; 

#endif 


B.  SOURCE  FILE 

#ifhdefMENU_C 
^define  MENU_C 
#include  "menu.H" 
^include  "time.H" 
#include  <iostream.h> 

long  mainmenu; 
long  hititem; 
short  value; 
double  wx,  wy; 


long  makethemenusO  /*  this  routine  performs  all  the  menu  construction  calls  */ 
long  topmenu; 

topmenu  =  defpup("Dynamics  Visualizer  %t  |  Exit  %x99"); 
retum(topmenu); 

} 


void  initializemenuO 

{ 

mainmenu  =  makethemenusQ; 

} 
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void  processmenuhit(long  hititem) 

{ 

switch(hititem) 

{ 

case  99: 

exit(O) 

break; 

default; 

break; 

}  /*  end  switch  */ 

} 


int  queue  testO 

{ 

hititem  =  0; 

while(qtestO) 

{ 

switch(qread(&value)) 

{ 

case  MENUBUTTON; 
if(  value  =  1) 

{ 


inmode(MSINGLE); 
hititem  =  dopup(mainmenu); 
mmode(MVIEWING); 
processmenuhit(hititem); 
resettimeQ; 

} 

break; 


case  LEFTMOUSE: 

wx  =  getvaluator(MOUSEX); 
wy  =  getvaluator^OUSEY); 
hititem  =  (wx  *  100000)  +  wy; 

break; 


case  REDRAW; 

reshapeviewportO; 

break; 
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case  UPARROWKEY 
hititem  =  1 00, 
break, 

case  DOWNARROWKEY 
hititem  =101, 
break; 

case  LEFTARROWKEY: 
hititem  =  102; 
break; 

case  RIGHT ARROWKEY 
hititem  =103, 
break; 

case  EQUALKEY: 
hititem  =  104; 
break; 

case  MINUSKEY: 
hititem  =  105; 
break; 

case  SPACEKEY; 

hititem  =  106; 
break; 

caseFlKEY: 

hititem  =  111; 
break; 

case  F2KEY: 

hititem  =112; 
break; 

case  F3KEY: 

hititem  =  113; 
break; 

caseF4KEY; 

hititem  =114; 
break. 
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case  F5KEY 

hititem  =115, 
break, 

case  F6KEY: 

hititem  =116; 
break, 

case  F7KEY : 

hititem  =117; 
break; 

case  F8KEY 

hititem  =118; 
break; 

case  F9KEY: 

hititem  =  119; 
break; 

caseFlOKEY: 

hititem  =  120; 
break; 

caseFllKEY. 

hititem  =  121; 
break; 

caseF12KEY: 

hititem  =  122; 
break; 


default; 

break; 

} 

} 

return  (int)  hititem; 


#endif 
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APPENDIX  H.  TIME  CODE 


A.  HEADER  FILE 

#ifhdef  TEMEH 
#define  TIME_H 
#include  <time.h> 

#include  <sys/types.h> 

#include  <sys/times.h> 

#include  <sys/param.h> 

void  set_delta(), 
void  set_delta(double); 
void  set  time(): 
void  reset_iime(j; 
double  read_delta(); 
double  readtimeO, 
int  read  ticksO, 

void  set_real_time_factor(double); 
#endif 


B.  SOURCE  FILE 

#ifhdefTIME_C 
#defirie  TIMEC 
#include  "time.H" 

struct  tms  timebuflfer; 

long  old  time;  double  delta,  real_tune_ratio  =  1.0; 

void  set  deltaO 

{ 

delta  =  ((double)  (times(^rtimebuflfer)  -  old_time)/HZ)  *  realjime  ratio; 
oldtime  =  times(&timebuffer); 

} 


void  set_delta(double  step) 

{ 

delta  =  step; 

oldtime  =  times(&timebufiFer); 

} 
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void  set_time() 

{ 

oldtime  =  times(&timebufFer); 

} 


double  read  deltaO 

{ 

return  delta; 

} 


double  readtimeO 

{ 

return  (double)  old  time; 

} 


int  readticksO 

{ 

return  (int)  times(&timebufFer); 

} 


void  set_real_time_factor(double  f) 

{ 

realtimeratio  =  f; 

} 


void  reset_timeO 

{ 

long  deltaticks; 

delta_ticks  =  (long)  (delta  *  HZ); 
old_time  =  times(&timebufifer)  -  delta_ticks; 

} 

#endif 
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APPENDIX  I.  ALTERING  THE  GRAVITY  GRADIENT  VISUALIZER  CODE 


Altering  the  Gravity  Gradient  Visualizer  code  is  a  tedious  process  not  to  be 
attempted  unless  one  has  a  working  knowledge  of  *he  UNIX  operating  system,  computer 
graphics  and  0++  programming.  If  the  user  feels  the  need  to  make  changes  to  the  code, 
the  following  steps  should  be  followed: 

•  After  logging  on  to  a  Silicon  Graphics  computer,  at  the  UNIX  prompt  type 
"cd  -jstewart/thesis".  This  will  put  the  user  in  the  directory  where  the  code 
resides. 

•  Using  any  text  editor,  edit  the  file  to  be  changed,  make  and  save  the  changes, 
and  exit  the  editor. 

•  At  the  UNIX  prompt  type  "make".  This  will  run  a  makefile  which  will  determine 
that  there  is  an  update  to  a  file  and  recompile  and  relink  the  file. 

•  Run  the  program  as  usual. 

As  an  example,  to  change  the  default  click  value  for  the  mass  field  from  50  kilogram 
increments  to  10  kilogram  increments,  log  on,  change  to  the  proper  directory  and  edit  the 
file  "ggrad.C".  Page  down  the  file  until  you  find  a  line  that  reads  "mass  =  mass  +  50;"  and 
change  the  "50"  to  "10".  Then  find  a  line  that  reads  "mass  =  mass  -  50;"  and  change  the 
"50"  to  "10".  Save  the  changes  and  exit  the  editor.  At  the  UNIX  prompt  type  "make"  to 
recompile  and  relink  the  code.  Run  the  program  as  usual.  This  process  can  be  repeated 
for  any  changes  one  ^\ashes  to  make.  Although  this  is  a  straightforward  process,  there  are 
many  opportunities  for  error,  and  these  errors  can  be  difiBcult  to  find.  Therefore,  it  can 
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not  be  emphasized  enough  that  this  should  not  be  attempted  by  one  without  the  requisite 
background  in  the  aforementioned  areas. 
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